├── base ├── srv │ └── Control.srv ├── package.xml └── CMakeLists.txt ├── bluesea-ros2 ├── params │ ├── other │ ├── uart_lidar.yaml │ ├── vpc_lidar.yaml │ ├── LDS-U50C-S.yaml │ ├── LDS-U80C-S.yaml │ ├── dual_udp_lidar.yaml │ └── udp_lidar.yaml ├── script │ ├── LHLiDAR.rules │ └── help.txt ├── package.xml ├── sdk │ ├── include │ │ ├── algorithmAPI.h │ │ ├── alarm.h │ │ ├── bluesea.h │ │ ├── parser.h │ │ ├── reader.h │ │ ├── usererror.h │ │ └── common_data.h │ └── src │ │ ├── uart.c │ │ ├── tcp_reader.cpp │ │ ├── algorithmAPI.cpp │ │ ├── bluesea.cpp │ │ ├── udp_reader.cpp │ │ ├── uart_reader.cpp │ │ └── parser.cpp ├── launch │ ├── heart_check.launch │ ├── LDS-U50C-S.launch │ ├── LDS-U80C-S.launch │ ├── uart_lidar.launch │ ├── udp_lidar.launch │ ├── vpc_lidar.launch │ └── dual_udp_lidar.launch ├── src │ ├── client.cpp │ ├── heart_check.cpp │ └── node.cpp ├── CMakeLists.txt └── rviz │ └── demo.rviz ├── README_CN.md └── README.md /base/srv/Control.srv: -------------------------------------------------------------------------------- 1 | string topic 2 | string func 3 | int8 flag 4 | string params 5 | --- 6 | int32 code 7 | string value -------------------------------------------------------------------------------- /bluesea-ros2/params/other: -------------------------------------------------------------------------------- 1 | 2 | 3 | #FITTER# 4 | # filter_open: false #滤波生效开关 5 | # max_range: 20.0 #滤波生效的最大距离 6 | # min_range: 0.5 #滤波生效的最小距离 7 | # max_range_difference: 0.1 #离异点判断的物理范围 8 | # filter_window: 1 #判断离异点下标的范围 -------------------------------------------------------------------------------- /bluesea-ros2/script/LHLiDAR.rules: -------------------------------------------------------------------------------- 1 | KERNEL=="ttyUSB*", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="7523", MODE:="0666", SYMLINK+="LHLiDAR" 2 | KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", MODE:="0666", SYMLINK+="LHLiDAR" 3 | -------------------------------------------------------------------------------- /bluesea-ros2/script/help.txt: -------------------------------------------------------------------------------- 1 | 1.lsusb 2 | Find the list of serial ports that require fixed mapping and find the VID and PID 3 | 2.vim /etc/udev/rules.d 4 | open the file that need to edit,and Content reference LHLiDAR.rules 5 | 3.sudo service udev reload 6 | 4.sudo service udev restart 7 | 8 | 9 | 10 | for example 11 | cmd：lsusb 12 | print:Bus 003 Device 007: ID 1a86:7523 QinHeng Electronics HL-340 USB-Serial adapter 13 | need write:KERNEL=="ttyUSB*", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="7523", MODE:="0666", SYMLINK+="LHLiDAR" -------------------------------------------------------------------------------- /bluesea-ros2/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | bluesea2 4 | 1.0.0 5 | The bluesea2 package 6 | zr 7 | TODO 8 | 9 | ament_cmake 10 | 11 | rclcpp 12 | rclcpp_components 13 | std_msgs 14 | sensor_msgs 15 | geometry_msgs 16 | base 17 | 18 | 19 | ament_cmake 20 | 21 | 22 | -------------------------------------------------------------------------------- /bluesea-ros2/sdk/include/algorithmAPI.h: -------------------------------------------------------------------------------- 1 | #ifndef _ALGORITHMAPI_ 2 | #define _ALGORITHMAPI_ 3 | #include"common_data.h" 4 | 5 | 6 | //离异点过滤 7 | bool checkWindowValid2(std::vectorscan, size_t idx, size_t window, double max_distance,double angle_increment); 8 | bool filter(std::vector& output_scan,double max_range,double min_range,double max_range_difference,unsigned int filter_window,double angle_increment); 9 | //判定一圈的点数中距离为0的点位的比例是否符合要求 10 | bool checkZeroDistance(std::vectorcomsume,float error_scale); 11 | //CRC算法 12 | unsigned int stm32crc(unsigned int *ptr, unsigned int len); 13 | //软采样设置 14 | void resample(RawData *dat, int NN); 15 | #endif 16 | -------------------------------------------------------------------------------- /base/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | base 5 | 0.0.0 6 | TODO: Package description 7 | spragunr 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | 12 | ament_lint_auto 13 | ament_lint_common 14 | 15 | rosidl_default_generators 16 | 17 | rosidl_default_runtime 18 | 19 | rosidl_interface_packages 20 | 21 | 22 | ament_cmake 23 | 24 | 25 | -------------------------------------------------------------------------------- /base/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(base) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++14 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 17) 12 | endif() 13 | 14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 15 | add_compile_options(-Wall -Wextra -Wpedantic) 16 | endif() 17 | 18 | # find dependencies 19 | find_package(ament_cmake REQUIRED) 20 | find_package(geometry_msgs REQUIRED) 21 | 22 | find_package(rosidl_default_generators REQUIRED) 23 | 24 | rosidl_generate_interfaces(${PROJECT_NAME} 25 | "srv/Control.srv" 26 | ) 27 | 28 | if(BUILD_TESTING) 29 | find_package(ament_lint_auto REQUIRED) 30 | set(ament_cmake_copyright_FOUND TRUE) 31 | set(ament_cmake_cpplint_FOUND TRUE) 32 | ament_lint_auto_find_test_dependencies() 33 | 34 | endif() 35 | 36 | ament_package() -------------------------------------------------------------------------------- /bluesea-ros2/params/uart_lidar.yaml: -------------------------------------------------------------------------------- 1 | bluesea_node: 2 | ros__parameters: 3 | #ROS2# 4 | frame_id: "map" 5 | scan_topic: "scan" 6 | cloud_topic: "cloud" 7 | #DATA# 8 | dev_id: -1 9 | raw_bytes: 3 10 | output_360: true 11 | output_scan: true 12 | output_cloud2: false 13 | with_angle_filter: false 14 | min_angle: -3.1415926 15 | max_angle: 3.1415926 16 | # mask1: "0,3.14" 17 | # mask2: "-3,-2" 18 | hard_resample: false 19 | soft_resample: false 20 | with_checksum: true 21 | reversed: false 22 | min_dist: 0.1 23 | max_dist: 50.0 24 | error_circle: 3 25 | error_scale: 0.9 26 | #CONNECT# 27 | type: "uart" 28 | port: "/dev/ttyUSB0" 29 | baud_rate: -1 30 | #GET# 31 | uuid: -1 32 | #SET# 33 | resample_res: -1.0 34 | with_confidence: -1 35 | with_smooth: -1 36 | with_deshadow: -1 37 | alarm_msg: -1 38 | rpm: -1 -------------------------------------------------------------------------------- /bluesea-ros2/sdk/src/uart.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | // #include 8 | 9 | int ioctl(int fd, unsigned long request, ...); 10 | 11 | int change_baud(int fd, unsigned int baud) 12 | { 13 | struct termios2 t; 14 | if (ioctl(fd, TCGETS2, &t)) 15 | { 16 | return -1; 17 | } 18 | t.c_cflag &= ~CBAUD; 19 | t.c_cflag |= BOTHER; 20 | t.c_ispeed = baud; 21 | t.c_ospeed = baud; 22 | 23 | if (ioctl(fd, TCSETS2, &t)) 24 | { 25 | return -2; 26 | } 27 | 28 | #if 1 29 | if (ioctl(fd, TCGETS2, &t) == 0) 30 | { 31 | if (abs((int)(t.c_ospeed - baud)) <= 20000) 32 | { 33 | //printf("reported %d\n", t.c_ospeed); 34 | } 35 | else 36 | { 37 | //printf("reported err set:%d result:%d\n", baud, t.c_ospeed); 38 | return -3; 39 | } 40 | } 41 | #endif 42 | 43 | return 0; 44 | } 45 | -------------------------------------------------------------------------------- /bluesea-ros2/params/vpc_lidar.yaml: -------------------------------------------------------------------------------- 1 | bluesea_node: 2 | ros__parameters: 3 | #ROS2 4 | frame_id: "map" 5 | scan_topic: "scan" 6 | cloud_topic: "cloud" 7 | #DATA# 8 | dev_id: -1 9 | raw_bytes: 3 10 | output_360: true 11 | output_scan: true 12 | output_cloud2: false 13 | with_angle_filter: false 14 | min_angle: -3.14159 15 | max_angle: 3.14159 16 | # mask1: "0,3.14" 17 | # mask2: "-3,-2" 18 | hard_resample: false 19 | soft_resample: false 20 | with_checksum: true 21 | reversed: false 22 | min_dist: 0.1 23 | max_dist: 50.0 24 | error_circle: 3 25 | error_scale: 0.9 26 | #CONNECT# 27 | type: "vpc" 28 | port: "/dev/ttyACM0" 29 | #GET# 30 | uuid: -1 31 | #SET# 32 | resample_res: -1.0 33 | with_confidence: -1 34 | with_smooth: -1 35 | with_deshadow: -1 36 | alarm_msg: -1 37 | rpm: -1 38 | direction: -1 39 | ats: 1 -------------------------------------------------------------------------------- /bluesea-ros2/params/LDS-U50C-S.yaml: -------------------------------------------------------------------------------- 1 | bluesea_node: 2 | ros__parameters: 3 | #ROS2# 4 | frame_id: "map" 5 | scan_topic: "scan" 6 | cloud_topic: "cloud" 7 | #DATA# 8 | dev_id: -1 9 | raw_bytes: 3 10 | output_360: true 11 | output_scan: true 12 | output_cloud2: false 13 | with_angle_filter: false 14 | min_angle: -1.0 15 | max_angle: 2.0 16 | # mask1: "0,3.14" 17 | # mask2: "-3,-2" 18 | hard_resample: false 19 | soft_resample: false 20 | with_checksum: true 21 | reversed: false 22 | min_dist: 0.01 23 | max_dist: 40.0 24 | error_circle: 3 25 | error_scale: 0.9 26 | with_confidence: 1 27 | unit_is_mm: 0 28 | #CONNECT# 29 | type: "udp" 30 | lidar_ip: "192.168.158.98" 31 | lidar_port: 6543 32 | local_port: 6668 33 | group_ip: "224.1.2.3" 34 | #GET# 35 | uuid: -1 36 | #SET# 37 | resample_res: -1.0 38 | with_confidence: -1 39 | with_smooth: -1 40 | with_deshadow: -1 41 | alarm_msg: -1 42 | rpm: -1 43 | direction: -1 44 | ats: -1 45 | -------------------------------------------------------------------------------- /bluesea-ros2/params/LDS-U80C-S.yaml: -------------------------------------------------------------------------------- 1 | bluesea_node: 2 | ros__parameters: 3 | #ROS2# 4 | frame_id: "map" 5 | scan_topic: "scan" 6 | cloud_topic: "cloud" 7 | #DATA# 8 | dev_id: -1 9 | raw_bytes: 3 10 | output_360: true 11 | output_scan: true 12 | output_cloud2: false 13 | with_angle_filter: false 14 | min_angle: -1.0 15 | max_angle: 2.0 16 | # mask1: "0,3.14" 17 | # mask2: "-3,-2" 18 | hard_resample: false 19 | soft_resample: false 20 | with_checksum: true 21 | reversed: false 22 | min_dist: 0.01 23 | max_dist: 80.0 24 | error_circle: 3 25 | error_scale: 0.9 26 | with_confidence: 1 27 | unit_is_mm: 0 28 | #CONNECT# 29 | type: "udp" 30 | lidar_ip: "192.168.158.98" 31 | lidar_port: 6543 32 | local_port: 6668 33 | group_ip: "224.1.2.3" 34 | #GET# 35 | uuid: -1 36 | #SET# 37 | resample_res: -1.0 38 | with_confidence: -1 39 | with_smooth: -1 40 | with_deshadow: -1 41 | alarm_msg: -1 42 | rpm: -1 43 | direction: -1 44 | ats: -1 45 | -------------------------------------------------------------------------------- /bluesea-ros2/params/dual_udp_lidar.yaml: -------------------------------------------------------------------------------- 1 | bluesea_node: 2 | ros__parameters: 3 | #ROS2# 4 | frame_id: "map" 5 | scan_topic: "scan" 6 | cloud_topic: "cloud" 7 | scan_topic1: "scan1" 8 | cloud_topic1: "cloud1" 9 | #DATA# 10 | dev_id: -1 11 | raw_bytes: 3 12 | output_360: true 13 | output_scan: true 14 | output_cloud2: false 15 | with_angle_filter: false 16 | min_angle: -3.14159 17 | max_angle: 3.14159 18 | # mask1: "0,3.14" 19 | # mask2: "-3,-2" 20 | hard_resample: false 21 | soft_resample: false 22 | with_checksum: true 23 | reversed: false 24 | min_dist: 0.1 25 | max_dist: 50.0 26 | error_circle: 3 27 | error_scale: 0.9 28 | #CONNECT# 29 | type: "udp" 30 | number: 2 31 | lidar_ip: "192.168.158.98" 32 | lidar_port: 6543 33 | lidar1_ip: "192.168.158.99" 34 | lidar1_port: 6543 35 | local_port: 6668 36 | group_ip: "224.1.2.3" 37 | #GET# 38 | uuid: -1 39 | #SET# 40 | resample_res: -1.0 41 | with_confidence: -1 42 | with_smooth: -1 43 | with_deshadow: -1 44 | alarm_msg: -1 45 | rpm: -1 46 | direction: -1 47 | ats: -1 48 | -------------------------------------------------------------------------------- /bluesea-ros2/params/udp_lidar.yaml: -------------------------------------------------------------------------------- 1 | bluesea_node: 2 | ros__parameters: 3 | #ROS2# 4 | frame_id: "map" 5 | scan_topic: "scan" 6 | cloud_topic: "cloud" 7 | #DATA# 8 | dev_id: -1 9 | raw_bytes: 3 10 | output_360: true 11 | output_scan: true 12 | output_cloud2: false 13 | with_angle_filter: false 14 | min_angle: -3.1415926 15 | max_angle: 3.1415926 16 | # mask1: "0,3.14" 17 | # mask2: "-3,-2" 18 | hard_resample: false 19 | soft_resample: false 20 | with_checksum: true 21 | reversed: false 22 | min_dist: 0.01 23 | max_dist: 50.0 24 | error_circle: 3 25 | error_scale: 0.9 26 | log_enable: false 27 | log_path: "/tmp/ros2_log.txt" 28 | #CONNECT# 29 | type: "udp" 30 | lidar_ip: "192.168.158.98" 31 | lidar_port: 6543 32 | local_port: 6668 33 | group_ip: "224.1.2.3" 34 | #GET# 35 | uuid: -1 36 | #SET# 37 | resample_res: -1.0 38 | with_confidence: -1 39 | with_smooth: -1 40 | with_deshadow: -1 41 | alarm_msg: -1 42 | rpm: -1 43 | direction: -1 44 | ats: -1 45 | #NTP 46 | ntp_ip: "192.168.158.50" 47 | ntp_port: 5678 48 | ntp_enable: -1 49 | -------------------------------------------------------------------------------- /bluesea-ros2/sdk/include/alarm.h: -------------------------------------------------------------------------------- 1 | #ifndef _ALARM_ 2 | #define _ALARM_ 3 | 4 | #define LMSG_ERROR 1 5 | #define LMSG_INFO 2 6 | #define LMSG_DEBUG 4 7 | #define LMSG_ALARM 0x100 8 | 9 | 10 | #define LERR_LOW_POWER 1 11 | #define LERR_MOTOR 2 12 | #define LERR_RANGER_HI 4 13 | #define LERR_NETWORK 8 14 | #define LERR_RANGER_IDLE 0x10 15 | #define LERR_ZERO_MISS 0x20 16 | #include 17 | struct LidarMsgHdr 18 | { 19 | char dev_sn[20]; 20 | uint32_t dev_id; // 21 | uint32_t timestamp;// 22 | uint32_t type; 23 | uint32_t data; 24 | }; 25 | 26 | struct LidarAlarm 27 | { 28 | char sign[4]; // must be "LMSG" 29 | uint32_t proto_version; // 0x101 30 | LidarMsgHdr hdr; 31 | uint16_t id; 32 | uint16_t extra; 33 | uint32_t zone_actived; 34 | uint8_t all_states[32]; 35 | uint32_t reserved[11]; 36 | }; 37 | #pragma pack (push,1) 38 | typedef struct 39 | { 40 | char sign[2]; /* AM */ 41 | uint8_t dev_id; /* 设备序号 */ 42 | uint32_t timestamp; /* 时间戳 */ 43 | uint16_t id; /* 消息序号 */ 44 | uint32_t events; /* 报警事件 */ 45 | uint32_t zone_actived; /* 当前激活防区 */ 46 | uint8_t reserved[11]; 47 | uint32_t crc; 48 | }PROCOTOL_HOST_ALARM_ST; 49 | #pragma pack (pop) 50 | #endif 51 | -------------------------------------------------------------------------------- /bluesea-ros2/sdk/include/bluesea.h: -------------------------------------------------------------------------------- 1 | #include"alarm.h" 2 | #include"algorithmAPI.h" 3 | #include"parser.h" 4 | #include"reader.h" 5 | #include"usererror.h" 6 | 7 | 8 | #define BLUESEA2_VERSION "2.6.4" 9 | 10 | class BlueSeaLidarDriver 11 | { 12 | public: 13 | BlueSeaLidarDriver(); 14 | ~BlueSeaLidarDriver(); 15 | //Get the instructions that need to be executed for initialization 16 | void getInitCmds(ArgData &argdata); 17 | void openLidarThread(); 18 | //void openHeartThread(); 19 | 20 | bool sendCmd(std::string topic,std::string cmd,int proto); 21 | bool sendUdpCmd2(char *ip, int len, char *cmd); 22 | 23 | PubHub*getHub(int i); 24 | Parser*getParser(int i); 25 | bool checkIsRun(int i); 26 | int GetAllFans(PubHub* pub, ArgData argdata, int8_t &counterclockwise); 27 | bool GetFan(PubHub* pub, bool with_resample, double resample_res, RawData **fans); 28 | 29 | double ROSAng(double ang); 30 | int GetCount(std::vector data, double min_deg, double max_deg, double &min_pos, double &max_pos); 31 | private: 32 | 33 | private: 34 | HReader m_reader;//NULL 35 | CommandList m_cmdlist;//init need run cmds 36 | //std::string m_type;//"uart" 37 | Parser *m_parsers[MAX_LIDARS]; 38 | PubHub *m_hubs[MAX_LIDARS]; 39 | ArgData m_argdata; 40 | int m_counterclockwise; 41 | }; 42 | bool judgepcIPAddrIsValid(const char *pcIPAddr); 43 | -------------------------------------------------------------------------------- /bluesea-ros2/launch/heart_check.launch: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | 3 | from ament_index_python.packages import get_package_share_directory 4 | 5 | from launch import LaunchDescription 6 | from launch_ros.actions import LifecycleNode 7 | from launch_ros.actions import Node 8 | from launch.actions import DeclareLaunchArgument 9 | from launch.substitutions import LaunchConfiguration 10 | from launch.actions import LogInfo 11 | 12 | import lifecycle_msgs.msg 13 | import os 14 | 15 | 16 | def generate_launch_description(): 17 | share_dir = get_package_share_directory('bluesea2') 18 | parameter_file = LaunchConfiguration('params_file') 19 | node_name = 'bluesea_heart_check' 20 | 21 | #对多版本的兼容性处理首字母e以及之前为18.04或更早,之后为20.04或更晚 22 | ROS_DISTRO='' 23 | ROS_DISTRO = os.getenv('ROS_DISTRO') 24 | print("Current ROS2 Version: ",ROS_DISTRO) 25 | if ROS_DISTRO[0] <= 'e': 26 | try: 27 | driver_node = LifecycleNode( node_name='bluesea_heart_check', node_namespace='/', package='bluesea2', node_executable='bluesea2_heart_check', output='screen') 28 | except: 29 | pass 30 | else : 31 | try: 32 | driver_node = LifecycleNode( name='bluesea_heart_check', namespace='/', package='bluesea2', executable='bluesea2_heart_check', output='screen', emulate_tty=True) 33 | except: 34 | pass 35 | 36 | 37 | return LaunchDescription([ 38 | driver_node 39 | ]) 40 | -------------------------------------------------------------------------------- /bluesea-ros2/sdk/include/parser.h: -------------------------------------------------------------------------------- 1 | #ifndef _PARSER_ 2 | #define _PARSER_ 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include"common_data.h" 11 | #include"usererror.h" 12 | #include"alarm.h" 13 | 14 | Parser* ParserOpen(int raw_bytes, bool with_chksum,int dev_id, 15 | int error_circle, double error_scale, bool from_zero,int time_mode,CommandList cmd, char *ip, int port); 16 | 17 | int strip(const char *s, char *buf); 18 | //int ParserClose(HParser); 19 | int ParserRunStream(Parser*, int len, unsigned char *buf, RawData *fans[]); 20 | int ParserRun(LidarNode, int len, unsigned char *buf, RawData *fans[]); 21 | 22 | void SetTimeStamp(RawData *); 23 | //void saveLog(const char *logPath, int type, const char *ip, const int port, const unsigned char *buf, unsigned int len); 24 | int mkpathAll(std::string s, mode_t mode); 25 | unsigned int stm32crc(unsigned int *ptr, unsigned int len); 26 | int alarmProc(unsigned char *buf, int len); 27 | //int autoGetFirstAngle(RawData raw, bool from_zero, std::vector &raws,std::string &result); 28 | 29 | std::string stringfilter(char *str, int num); 30 | int getFirstidx(RawData raw, int angle); 31 | void timestampMode(int type); 32 | 33 | //中断进程 34 | void closeSignal(int sig); 35 | // //指定编号雷达发送指令 36 | // bool SendCmd(int len, char *cmd, int index); 37 | // //获取一帧第一个点所在的扇区，以及该点所在扇区的位置 38 | // bool autoGetFirstAngle2(PubHub* pub, bool from_zero); 39 | //对读取的数据进行解析参数:实际获取的数据配置文件对应的参数 40 | //int GetAllFans(HPublish pub,ArgData argdata,uint8_t &counterclockwise); 41 | 42 | void PublishData(PubHub* pub, int n, RawData **fans); 43 | 44 | void redirect_stdout_to_log(const char *path) ; 45 | 46 | #endif 47 | -------------------------------------------------------------------------------- /bluesea-ros2/launch/LDS-U50C-S.launch: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | 3 | from ament_index_python.packages import get_package_share_directory 4 | 5 | from launch import LaunchDescription 6 | from launch_ros.actions import LifecycleNode 7 | from launch_ros.actions import Node 8 | from launch.actions import DeclareLaunchArgument 9 | from launch.substitutions import LaunchConfiguration 10 | from launch.actions import LogInfo 11 | 12 | import lifecycle_msgs.msg 13 | import os 14 | 15 | 16 | def generate_launch_description(): 17 | share_dir = get_package_share_directory('bluesea2') 18 | parameter_file = LaunchConfiguration('params_file') 19 | node_name = 'bluesea_node' 20 | 21 | params_declare = DeclareLaunchArgument('params_file', 22 | default_value=os.path.join( 23 | share_dir, 'params', 'LDS-U50C-S.yaml'), 24 | description='FPath to the ROS2 parameters file to use.') 25 | 26 | #对多版本的兼容性处理首字母e以及之前为18.04或更早,之后为20.04或更晚 27 | ROS_DISTRO='' 28 | ROS_DISTRO = os.getenv('ROS_DISTRO') 29 | print("Current ROS2 Version: ",ROS_DISTRO) 30 | if ROS_DISTRO[0] <= 'e': 31 | try: 32 | driver_node = LifecycleNode( node_name='bluesea_node', node_namespace='/', package='bluesea2', node_executable='bluesea2_node', output='screen', parameters=[parameter_file]) 33 | except: 34 | pass 35 | else : 36 | try: 37 | driver_node = LifecycleNode( name='bluesea_node', namespace='/', package='bluesea2', executable='bluesea2_node', output='screen', emulate_tty=True, parameters=[parameter_file]) 38 | except: 39 | pass 40 | 41 | 42 | return LaunchDescription([ 43 | params_declare, 44 | driver_node 45 | ]) 46 | -------------------------------------------------------------------------------- /bluesea-ros2/launch/LDS-U80C-S.launch: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | 3 | from ament_index_python.packages import get_package_share_directory 4 | 5 | from launch import LaunchDescription 6 | from launch_ros.actions import LifecycleNode 7 | from launch_ros.actions import Node 8 | from launch.actions import DeclareLaunchArgument 9 | from launch.substitutions import LaunchConfiguration 10 | from launch.actions import LogInfo 11 | 12 | import lifecycle_msgs.msg 13 | import os 14 | 15 | 16 | def generate_launch_description(): 17 | share_dir = get_package_share_directory('bluesea2') 18 | parameter_file = LaunchConfiguration('params_file') 19 | node_name = 'bluesea_node' 20 | 21 | params_declare = DeclareLaunchArgument('params_file', 22 | default_value=os.path.join( 23 | share_dir, 'params', 'LDS-U80C-S.yaml'), 24 | description='FPath to the ROS2 parameters file to use.') 25 | 26 | #对多版本的兼容性处理首字母e以及之前为18.04或更早,之后为20.04或更晚 27 | ROS_DISTRO='' 28 | ROS_DISTRO = os.getenv('ROS_DISTRO') 29 | print("Current ROS2 Version: ",ROS_DISTRO) 30 | if ROS_DISTRO[0] <= 'e': 31 | try: 32 | driver_node = LifecycleNode( node_name='bluesea_node', node_namespace='/', package='bluesea2', node_executable='bluesea2_node', output='screen', parameters=[parameter_file]) 33 | except: 34 | pass 35 | else : 36 | try: 37 | driver_node = LifecycleNode( name='bluesea_node', namespace='/', package='bluesea2', executable='bluesea2_node', output='screen', emulate_tty=True, parameters=[parameter_file]) 38 | except: 39 | pass 40 | 41 | 42 | return LaunchDescription([ 43 | params_declare, 44 | driver_node 45 | ]) 46 | -------------------------------------------------------------------------------- /bluesea-ros2/launch/uart_lidar.launch: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | 3 | from ament_index_python.packages import get_package_share_directory 4 | 5 | from launch import LaunchDescription 6 | from launch_ros.actions import LifecycleNode 7 | from launch_ros.actions import Node 8 | from launch.actions import DeclareLaunchArgument 9 | from launch.substitutions import LaunchConfiguration 10 | from launch.actions import LogInfo 11 | 12 | import lifecycle_msgs.msg 13 | import os 14 | 15 | 16 | def generate_launch_description(): 17 | share_dir = get_package_share_directory('bluesea2') 18 | parameter_file = LaunchConfiguration('params_file') 19 | node_name = 'bluesea_node' 20 | 21 | params_declare = DeclareLaunchArgument('params_file', 22 | default_value=os.path.join( 23 | share_dir, 'params', 'uart_lidar.yaml'), 24 | description='FPath to the ROS2 parameters file to use.') 25 | 26 | #对多版本的兼容性处理首字母e以及之前为18.04或更早,之后为20.04或更晚 27 | ROS_DISTRO='' 28 | ROS_DISTRO = os.getenv('ROS_DISTRO') 29 | print("Current ROS2 Version: ",ROS_DISTRO) 30 | if ROS_DISTRO[0] <= 'e': 31 | try: 32 | driver_node = LifecycleNode( node_name='bluesea_node', node_namespace='/', package='bluesea2', node_executable='bluesea2_node', output='screen', parameters=[parameter_file]) 33 | except: 34 | pass 35 | else : 36 | try: 37 | driver_node = LifecycleNode( name='bluesea_node', namespace='/', package='bluesea2', executable='bluesea2_node', output='screen', emulate_tty=True, parameters=[parameter_file]) 38 | except: 39 | pass 40 | 41 | 42 | return LaunchDescription([ 43 | params_declare, 44 | driver_node 45 | ]) 46 | -------------------------------------------------------------------------------- /bluesea-ros2/launch/udp_lidar.launch: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | 3 | from ament_index_python.packages import get_package_share_directory 4 | 5 | from launch import LaunchDescription 6 | from launch_ros.actions import LifecycleNode 7 | from launch_ros.actions import Node 8 | from launch.actions import DeclareLaunchArgument 9 | from launch.substitutions import LaunchConfiguration 10 | from launch.actions import LogInfo 11 | 12 | import lifecycle_msgs.msg 13 | import os 14 | 15 | 16 | def generate_launch_description(): 17 | share_dir = get_package_share_directory('bluesea2') 18 | parameter_file = LaunchConfiguration('params_file') 19 | node_name = 'bluesea_node' 20 | 21 | params_declare = DeclareLaunchArgument('params_file', 22 | default_value=os.path.join( 23 | share_dir, 'params', 'udp_lidar.yaml'), 24 | description='FPath to the ROS2 parameters file to use.') 25 | 26 | #对多版本的兼容性处理首字母e以及之前为18.04或更早,之后为20.04或更晚 27 | ROS_DISTRO='' 28 | ROS_DISTRO = os.getenv('ROS_DISTRO') 29 | print("Current ROS2 Version: ",ROS_DISTRO) 30 | if ROS_DISTRO[0] <= 'e': 31 | try: 32 | driver_node = LifecycleNode( node_name='bluesea_node', node_namespace='/', package='bluesea2', node_executable='bluesea2_node', output='screen', parameters=[parameter_file]) 33 | except: 34 | pass 35 | else : 36 | try: 37 | driver_node = LifecycleNode( name='bluesea_node', namespace='/', package='bluesea2', executable='bluesea2_node', output='screen', emulate_tty=True, parameters=[parameter_file]) 38 | except: 39 | pass 40 | 41 | 42 | return LaunchDescription([ 43 | params_declare, 44 | driver_node 45 | ]) 46 | -------------------------------------------------------------------------------- /bluesea-ros2/launch/vpc_lidar.launch: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | 3 | from ament_index_python.packages import get_package_share_directory 4 | 5 | from launch import LaunchDescription 6 | from launch_ros.actions import LifecycleNode 7 | from launch_ros.actions import Node 8 | from launch.actions import DeclareLaunchArgument 9 | from launch.substitutions import LaunchConfiguration 10 | from launch.actions import LogInfo 11 | 12 | import lifecycle_msgs.msg 13 | import os 14 | 15 | 16 | def generate_launch_description(): 17 | share_dir = get_package_share_directory('bluesea2') 18 | parameter_file = LaunchConfiguration('params_file') 19 | node_name = 'bluesea_node' 20 | 21 | params_declare = DeclareLaunchArgument('params_file', 22 | default_value=os.path.join( 23 | share_dir, 'params', 'vpc_lidar.yaml'), 24 | description='FPath to the ROS2 parameters file to use.') 25 | 26 | #对多版本的兼容性处理首字母e以及之前为18.04或更早,之后为20.04或更晚 27 | ROS_DISTRO='' 28 | ROS_DISTRO = os.getenv('ROS_DISTRO') 29 | print("Current ROS2 Version: ",ROS_DISTRO) 30 | if ROS_DISTRO[0] <= 'e': 31 | try: 32 | driver_node = LifecycleNode( node_name='bluesea_node', node_namespace='/', package='bluesea2', node_executable='bluesea2_node', output='screen', parameters=[parameter_file]) 33 | except: 34 | pass 35 | else : 36 | try: 37 | driver_node = LifecycleNode( name='bluesea_node', namespace='/', package='bluesea2', executable='bluesea2_node', output='screen', emulate_tty=True, parameters=[parameter_file]) 38 | except: 39 | pass 40 | 41 | 42 | return LaunchDescription([ 43 | params_declare, 44 | driver_node 45 | ]) 46 | -------------------------------------------------------------------------------- /bluesea-ros2/launch/dual_udp_lidar.launch: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | 3 | from ament_index_python.packages import get_package_share_directory 4 | 5 | from launch import LaunchDescription 6 | from launch_ros.actions import LifecycleNode 7 | from launch_ros.actions import Node 8 | from launch.actions import DeclareLaunchArgument 9 | from launch.substitutions import LaunchConfiguration 10 | from launch.actions import LogInfo 11 | 12 | import lifecycle_msgs.msg 13 | import os 14 | 15 | 16 | def generate_launch_description(): 17 | share_dir = get_package_share_directory('bluesea2') 18 | parameter_file = LaunchConfiguration('params_file') 19 | node_name = 'bluesea_node' 20 | 21 | params_declare = DeclareLaunchArgument('params_file', 22 | default_value=os.path.join( 23 | share_dir, 'params', 'dual_udp_lidar.yaml'), 24 | description='FPath to the ROS2 parameters file to use.') 25 | 26 | #对多版本的兼容性处理首字母e以及之前为18.04或更早,之后为20.04或更晚 27 | ROS_DISTRO='' 28 | ROS_DISTRO = os.getenv('ROS_DISTRO') 29 | print("Current ROS2 Version: ",ROS_DISTRO) 30 | if ROS_DISTRO[0] <= 'e': 31 | try: 32 | driver_node = LifecycleNode( node_name='bluesea_node', node_namespace='/', package='bluesea2', node_executable='bluesea2_node', output='screen', parameters=[parameter_file]) 33 | except: 34 | pass 35 | else : 36 | try: 37 | driver_node = LifecycleNode( name='bluesea_node', namespace='/', package='bluesea2', executable='bluesea2_node', output='screen', emulate_tty=True, parameters=[parameter_file]) 38 | except: 39 | pass 40 | 41 | 42 | return LaunchDescription([ 43 | params_declare, 44 | driver_node 45 | ]) 46 | -------------------------------------------------------------------------------- /bluesea-ros2/sdk/include/reader.h: -------------------------------------------------------------------------------- 1 | #ifndef _READER_ 2 | #define _READER_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include "parser.h" 23 | 24 | HReader StartUartReader(const char* type ,const char* port, int baudrate, Parser*hParser, PubHub*); 25 | bool SendUartCmd(HReader, int len, char*); 26 | bool SendVpcCmd(HReader hr, int len, char* cmd); 27 | 28 | HReader StartUDPReader(const char* type,unsigned short listen_port, bool is_group_listener, const char* group_ip, 29 | int lidar_count, const LidarInfo* lidars); 30 | 31 | 32 | bool SendUdpCmd(HReader hr, std::string lidarIp,int port,std::string cmd,int proto); 33 | HReader StartTCPReader(const char* lidar_ip, unsigned short lidar_port, Parser* hParser, PubHub* hPub); 34 | 35 | bool SendTcpCmd(HReader hr, int len, char* cmd); 36 | 37 | void StopUartReader(HReader hr); 38 | void StopUDPReader(HReader hr); 39 | void StopTCPReader(HReader hr); 40 | 41 | 42 | //UART 43 | int open_serial_port(const char *port, int baudrate); 44 | bool uart_talk(int fd, int n, const char* cmd, int nhdr, const char* hdr_str, int nfetch, char* fetch,int waittime=10,int cachelength=4096); 45 | int GetComBaud(std::string uartname); 46 | 47 | 48 | //VPC 49 | bool vpc_talk(int hcom, int mode, short sn, int len, const char* cmd, int nfetch, void* result); 50 | 51 | 52 | //udp 53 | bool setup_lidar_udp(int handle,Parser* hP, EEpromV101 ¶m); 54 | void send_cmd_udp(int fd_udp, const char* dev_ip, int dev_port, int cmd, int sn, int len, const void* snd_buf); 55 | bool udp_talk_S_PACK(int fd_udp, const char* ip, int port, int n, const char* cmd, void* result); 56 | bool udp_talk_C_PACK(int fd_udp, const char* lidar_ip, int lidar_port,int n, const char* cmd,int nhdr, const char* hdr_str,int nfetch, char* fetch); 57 | bool udp_talk_GS_PACK(int fd_udp, const char *ip, int port, int n, const char *cmd, void *result); 58 | 59 | //common 60 | void PublishData(PubHub* pub, int n, RawData **fans); 61 | 62 | #endif 63 | -------------------------------------------------------------------------------- /bluesea-ros2/src/client.cpp: -------------------------------------------------------------------------------- 1 | #include "rclcpp/clock.hpp" 2 | #include "rclcpp/rclcpp.hpp" 3 | #include "rclcpp/time_source.hpp" 4 | #include "sensor_msgs/msg/laser_scan.hpp" 5 | #include "base/srv/control.hpp" 6 | int main(int argc, char **argv) 7 | { 8 | if (argc != 3&&argc != 4) 9 | { 10 | RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"usage: ros2 run bluesea2 bluesea2_client scan start arg1 is topic arg2 is action(start/stop)"); 11 | RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"usage: ros2 run bluesea2 bluesea2_client scan switchZone 0 arg1 is topic arg2 is action(switchZone) arg3 is select zone id"); 12 | RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"usage: ros2 run bluesea2 bluesea2_client scan rpm 600 arg1 is topic arg2 is action(set rpm) arg3 is rpm value"); 13 | RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"usage: ros2 run bluesea2 bluesea2_client heart check 1 arg1 is service name arg2 is action(check) arg3 is print / not print"); 14 | return 1; 15 | } 16 | rclcpp::init(argc, argv); 17 | std::shared_ptr node = rclcpp::Node::make_shared("bluesea_node_client"); 18 | char serviceName[64] = {0}; 19 | sprintf(serviceName,"/%s_motor",argv[1]); 20 | 21 | rclcpp::Client::SharedPtr client = node->create_client(serviceName); // CHANGE 22 | //Retain four universal parameters 23 | auto request = std::make_shared(); 24 | request->topic=argv[1]; 25 | request->func=argv[2]; 26 | if(strcmp(argv[2],"start")==0||strcmp(argv[2],"stop")==0) 27 | { 28 | } 29 | else if(strcmp(argv[2],"switchZone")==0) 30 | { 31 | request->flag=atoi(argv[3]); 32 | } 33 | else if(strcmp(argv[2],"rpm")==0) 34 | { 35 | request->params=argv[3]; 36 | 37 | } 38 | else if(strcmp(argv[2],"check")==0) 39 | { 40 | request->flag=atoi(argv[3]); 41 | } 42 | 43 | while (!client->wait_for_service(std::chrono::seconds(1))) 44 | { 45 | if (!rclcpp::ok()) 46 | { 47 | RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service. Exiting."); 48 | return 0; 49 | } 50 | RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "service not available, waiting again..."); 51 | } 52 | 53 | auto result = client->async_send_request(request); 54 | #ifdef ROS_DISTRO_F 55 | if(rclcpp::spin_until_future_complete(node, result)==rclcpp::FutureReturnCode::SUCCESS) 56 | #elif defined ROS_DISTRO_E 57 | //ros2 eloquent 18.04 and lower 58 | if(rclcpp::spin_until_future_complete(node, result)==rclcpp::executor::FutureReturnCode::SUCCESS) 59 | #endif 60 | { 61 | RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "client OK"); 62 | } 63 | else 64 | { 65 | RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to call ctrl_service "); // CHANGE 66 | } 67 | 68 | rclcpp::shutdown(); 69 | return 0; 70 | } 71 | 72 | -------------------------------------------------------------------------------- /bluesea-ros2/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(bluesea2) 3 | 4 | 5 | # Default to C99 6 | if(NOT CMAKE_C_STANDARD) 7 | set(CMAKE_C_STANDARD 99) 8 | endif() 9 | 10 | # Default to C++14 11 | if(NOT CMAKE_CXX_STANDARD) 12 | set(CMAKE_CXX_STANDARD 17) 13 | endif() 14 | 15 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 16 | add_compile_options(-Wall -Wextra -Wpedantic) 17 | endif() 18 | 19 | # Get ROS2 version information (initial letter) from environment variables 20 | string(SUBSTRING $ENV{ROS_DISTRO} 0 1 tmp) 21 | #MESSAGE("ROS_DISTRO2:${tmp}") 22 | 23 | string(COMPARE LESS_EQUAL ${tmp} "e" flag) 24 | #MESSAGE("flag:${flag}") 25 | 26 | 27 | if( ${flag} STREQUAL 1) 28 | add_definitions(-DROS_DISTRO_E) 29 | else() 30 | add_definitions(-DROS_DISTRO_F) 31 | endif() 32 | 33 | # find dependencies 34 | find_package(ament_cmake REQUIRED) 35 | #find_package(rmw REQUIRED) 36 | find_package(rclcpp REQUIRED) 37 | find_package(sensor_msgs REQUIRED) 38 | find_package(visualization_msgs REQUIRED) 39 | find_package(geometry_msgs REQUIRED) 40 | find_package(base REQUIRED) 41 | find_package(std_srvs REQUIRED) 42 | 43 | include_directories(${sensor_msgs_INCLUDE_DIRS}) 44 | 45 | set(BLUESEA_SDK_PATH "./sdk/") 46 | FILE(GLOB BLUESEA_SDK_SRC "${BLUESEA_SDK_PATH}/src/*.cpp" "${BLUESEA_SDK_PATH}/src/*.c") 47 | 48 | 49 | 50 | add_executable(${PROJECT_NAME}_node ./src/node.cpp ${BLUESEA_SDK_SRC}) 51 | add_executable(${PROJECT_NAME}_client ./src/client.cpp) 52 | add_executable(${PROJECT_NAME}_heart_check ./src/heart_check.cpp) 53 | 54 | 55 | ament_target_dependencies(${PROJECT_NAME}_node rclcpp sensor_msgs visualization_msgs std_srvs geometry_msgs base) 56 | ament_target_dependencies(${PROJECT_NAME}_client rclcpp geometry_msgs base) 57 | ament_target_dependencies(${PROJECT_NAME}_heart_check rclcpp geometry_msgs std_srvs base) 58 | 59 | 60 | 61 | # target_compile_features(${PROJECT_NAME}_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 62 | # target_compile_features(${PROJECT_NAME}_client PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 63 | # target_compile_features(${PROJECT_NAME}_heart_check PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 64 | 65 | 66 | # target_include_directories(${PROJECT_NAME}_node PUBLIC 67 | # $ 68 | # $) 69 | 70 | 71 | # target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES}) 72 | # target_link_libraries(${PROJECT_NAME}_client ${catkin_LIBRARIES}) 73 | # target_link_libraries(${PROJECT_NAME}_heart_check ${catkin_LIBRARIES}) 74 | 75 | install(TARGETS ${PROJECT_NAME}_node ${PROJECT_NAME}_client ${PROJECT_NAME}_heart_check DESTINATION lib/${PROJECT_NAME}) 76 | install(DIRECTORY launch params DESTINATION share/${PROJECT_NAME}) 77 | 78 | if(BUILD_TESTING) 79 | find_package(ament_lint_auto REQUIRED) 80 | ament_lint_auto_find_test_dependencies() 81 | endif() 82 | ament_package() 83 | 84 | -------------------------------------------------------------------------------- /bluesea-ros2/sdk/src/tcp_reader.cpp: -------------------------------------------------------------------------------- 1 | #include "../include/reader.h" 2 | 3 | struct TCPInfo 4 | { 5 | Parser* hParser; 6 | PubHub* hPublish; 7 | int fd_tcp; 8 | int lidar_port; 9 | char lidar_ip[32]; 10 | //int listen_port; 11 | //char group_ip[32]; 12 | pthread_t thr; 13 | }; 14 | 15 | int tcp_reader(int sock, Parser* hParser, PubHub* hPublish) 16 | { 17 | RawData* fans[MAX_FANS]; 18 | 19 | while (1) 20 | { 21 | fd_set fds; 22 | FD_ZERO(&fds); 23 | 24 | FD_SET(sock, &fds); 25 | 26 | struct timeval to = { 5, 5 }; 27 | int ret = select(sock+1, &fds, NULL, NULL, &to); 28 | 29 | if (ret == 0) { 30 | 31 | } 32 | 33 | if (ret < 0) { 34 | printf("select error\n"); 35 | continue; 36 | } 37 | 38 | // read TCP data 39 | if (FD_ISSET(sock, &fds)) 40 | { 41 | unsigned char buf[1024]; 42 | int nr = recv(sock, buf, sizeof(buf), 0); 43 | if (nr <= 0) { 44 | printf("read tcp error %d\n", nr); 45 | break; 46 | } 47 | else 48 | { 49 | int nfan = ParserRunStream(hParser, nr, buf, &(fans[0])); 50 | if (nfan > 0) { 51 | PublishData(hPublish, nfan, fans); 52 | } 53 | } 54 | } 55 | } 56 | 57 | return 0; 58 | } 59 | 60 | void* TcpThreadProc(void* p) 61 | { 62 | TCPInfo* info = (TCPInfo*)p; 63 | 64 | while (1) 65 | { 66 | // open TCP 67 | int sock = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP); 68 | if (sock < 0) { 69 | printf("socket TCP failed\n"); 70 | return 0; 71 | } 72 | 73 | struct sockaddr_in addr; 74 | memset(&addr, 0, sizeof(addr)); /* Zero out structure */ 75 | addr.sin_family = AF_INET; /* Internet address family */ 76 | 77 | addr.sin_addr.s_addr = inet_addr(info->lidar_ip); 78 | addr.sin_port = htons(info->lidar_port); 79 | 80 | int ret = connect(sock, (struct sockaddr *) &addr, sizeof(addr)); 81 | 82 | if (ret != 0) 83 | { 84 | printf("connect (%s:%d) failed", info->lidar_ip, info->lidar_port); 85 | close(sock); 86 | //sleep(15); 87 | continue; 88 | } 89 | info->fd_tcp = sock; 90 | printf("connect (%s:%d) ok", info->lidar_ip, info->lidar_port); 91 | 92 | tcp_reader(sock, info->hParser, info->hPublish); 93 | 94 | printf("disconnect (%s:%d)\n", info->lidar_ip, info->lidar_port); 95 | 96 | info->fd_tcp = 0; 97 | close(sock); 98 | } 99 | return NULL; 100 | } 101 | 102 | #if 0 103 | void* TcpSrvProc(void* p) 104 | { 105 | TCPInfo* info = (TCPInfo*)p; 106 | 107 | // open TCP port 108 | int sock = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP); 109 | sockaddr_in addr; 110 | addr.sin_family = AF_INET; 111 | addr.sin_port = htons(info->listen_port); 112 | addr.sin_addr.s_addr = htonl(INADDR_ANY); 113 | 114 | int rt = ::bind(info->fd_tcp, (struct sockaddr *)&addr, sizeof(addr)); 115 | 116 | if (rt != 0) 117 | { 118 | printf("bind port %d failed\n", info->listen_port); 119 | return NULL; 120 | } 121 | 122 | printf("start tcp server %s:%d\n", lidar_ip, lidar_port); 123 | 124 | 125 | while (1) { 126 | 127 | 128 | } 129 | 130 | return NULL; 131 | } 132 | 133 | 134 | HReader StartTCPServer(unsigned short listen_port, HParser hParser, HPublish hPub) 135 | { 136 | signal(SIGPIPE, SIG_IGN); 137 | 138 | TCPInfo* info = new TCPInfo; 139 | info->hParser = hParser; 140 | info->hPublish = hPub; 141 | info->listen_port = listen_port; 142 | 143 | pthread_create(&info->thr, NULL, TcpSrvProc, info); 144 | 145 | return info; 146 | } 147 | #endif 148 | 149 | HReader StartTCPReader(const char* lidar_ip, unsigned short lidar_port, Parser *hParser, PubHub* hPub) 150 | { 151 | signal(SIGPIPE, SIG_IGN); 152 | 153 | TCPInfo* info = new TCPInfo; 154 | info->hParser = hParser; 155 | info->hPublish = hPub; 156 | info->lidar_port = lidar_port; 157 | strcpy(info->lidar_ip, lidar_ip); 158 | //strcpy(info->group_ip, group_ip); 159 | //printf("start udp %s:%d udp %d\n", lidar_ip, lidar_port, info->fd_tcp); 160 | 161 | pthread_create(&info->thr, NULL, TcpThreadProc, info); 162 | 163 | return info; 164 | } 165 | 166 | bool SendTcpCmd(HReader hr, int len, char* cmd) 167 | { 168 | TCPInfo* info = (TCPInfo*)hr; 169 | if (!info || info->fd_tcp <= 0) 170 | return false; 171 | 172 | return send(info->fd_tcp, cmd, len, 0) == len; 173 | } 174 | 175 | 176 | void StopTCPReader(HReader hr) 177 | { 178 | TCPInfo* info = (TCPInfo*)hr; 179 | //info->should_exit = true; 180 | //sleep(1); 181 | pthread_join(info->thr, NULL); 182 | delete info; 183 | } 184 | -------------------------------------------------------------------------------- /bluesea-ros2/sdk/src/algorithmAPI.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "../include/algorithmAPI.h" 3 | #include 4 | #include 5 | 6 | bool checkWindowValid2(std::vector scan, size_t idx, size_t window, double max_distance,double angle_increment) 7 | { 8 | 9 | unsigned int num_neighbors = 0; 10 | const float r1 = scan.at(idx).distance; // 当前点云的距离数据 11 | float r2 = 0.; // 范围内点云的数据 12 | // Look around the current point until either the window is exceeded 13 | // or the number of neighbors was found. 14 | for (int y = -(int)window; y < (int)window + 1 && num_neighbors = static_cast(scan.size())||idx == (size_t)j) 18 | { 19 | continue; 20 | } 21 | r2 = scan.at(j).distance; 22 | if(r2==0) 23 | continue; 24 | // delete the re in the result 25 | const float d = sqrt(pow(r1, 2) + pow(r2, 2) -(2 * r1 * r2 * cosf(y * angle_increment))); 26 | if (d <= max_distance) 27 | { 28 | num_neighbors++; 29 | } 30 | } 31 | //printf("%s %d %d %d\n", __FUNCTION__, __LINE__,num_neighbors,window); 32 | // consider the window to be the number of neighbors we need 33 | if (num_neighbors < window) 34 | // if(num_neighbors == 0)//only the window is one 35 | { 36 | return false; // invalid 37 | } 38 | else 39 | { 40 | return true; // effective 41 | } 42 | } 43 | 44 | bool filter(std::vector &output_scan, double max_range, double min_range, double max_range_difference, unsigned int filter_window,double angle_increment) 45 | { 46 | std::vector valid_ranges; 47 | /*Check if range size is big enough to use7 the filter window */ 48 | if (output_scan.size() <= filter_window + 1) 49 | { 50 | printf("Scan ranges size is too small: size = %ld", output_scan.size()); 51 | return false; 52 | } 53 | size_t i = 0; 54 | size_t i_max = output_scan.size(); 55 | valid_ranges.clear(); 56 | while (i < i_max) 57 | { 58 | bool out_of_range = ((output_scan.at(i).distance > max_range) || (output_scan.at(i).distance < min_range)); 59 | // ROS_INFO("%lf", min_range); 60 | valid_ranges.push_back(out_of_range); 61 | ++i; 62 | } 63 | i = 0; 64 | i_max = output_scan.size() - filter_window + 1; 65 | while (i < i_max) 66 | { 67 | bool window_valid = checkWindowValid2(output_scan, i, filter_window, max_range_difference,angle_increment); 68 | 69 | if (window_valid) 70 | { 71 | size_t j = i, j_max = i + filter_window; 72 | do 73 | { 74 | valid_ranges[j++] = true; 75 | } while (j < j_max); 76 | } 77 | ++i; 78 | } 79 | i = 0; 80 | i_max = valid_ranges.size(); 81 | int errnum=0; 82 | while (i < i_max) 83 | { 84 | if (!valid_ranges[i]) 85 | { 86 | output_scan[i].distance = std::numeric_limits::infinity(); 87 | errnum++; 88 | } 89 | ++i; 90 | } 91 | //printf("%ld not valid num:%d\n",i,errnum); 92 | return true; 93 | } 94 | bool checkZeroDistance(std::vectordata,float error_scale) 95 | { 96 | int lengthZeroNum = 0; 97 | for(unsigned int i=0;i>= 1; 133 | } 134 | } 135 | return crc32; 136 | } 137 | 138 | 139 | void resample(RawData *dat, int NN) 140 | { 141 | int *index = new int[NN]; 142 | double *errs = new double[NN]; 143 | 144 | for (int i = 0; i < NN; i++) 145 | index[i] = -1; 146 | 147 | for (int i = 0; i < dat->N; i++) 148 | { 149 | if (dat->points[i].distance == 0) 150 | continue; 151 | 152 | int idx = round(double(i * NN) / dat->N); 153 | if (idx < NN) 154 | { 155 | double err = fabs(double(dat->span * i) / dat->N - double(dat->span * idx) / NN); 156 | if (index[idx] == -1 || err < errs[idx]) 157 | { 158 | index[idx] = i; 159 | errs[idx] = err; 160 | } 161 | } 162 | } 163 | 164 | for (int i = 1; i < NN; i++) 165 | { 166 | if (index[i] != -1) 167 | { 168 | dat->points[i] = dat->points[index[i]]; 169 | } 170 | else 171 | { 172 | dat->points[i].distance = 0; 173 | dat->points[i].confidence = 0; 174 | } 175 | dat->points[i].degree = dat->angle / 10.0 + (dat->span * i) / (10.0 * NN); 176 | } 177 | 178 | dat->N = NN; 179 | 180 | delete index; 181 | delete errs; 182 | } -------------------------------------------------------------------------------- /README_CN.md: -------------------------------------------------------------------------------- 1 | # 蓝海光电ROS2驱动程序V2.6(bluesea2_ros2_driver) 2 | 3 | ## 概述 4 | ---------- 5 | 蓝海光电ROS2驱动程序是专门用于连接本公司生产的lidar产品。该驱动程序可以在安装了 ROS2 环境的操作系统中运行，主要支持ubuntu系列操作系统（18.04LTS-最新）。经测试可以运行该ROS驱动程序的硬件平台包括：intel x86 主流 cpu 平台，部分 ARM64 硬件平台（如英伟达、瑞芯微，树莓派等，可能需要更新cp210x驱动）。 6 | 7 | ## 注意事项 8 | 请保证路径中不带中文字符，否则会编译失败 9 | 10 | ## 获取并构建蓝海ROS驱动包 11 | 1.从Github获取蓝海ROS2驱动程序,并部署对应位置 12 | 13 | mkdir bluesea2 //创建一个文件夹，自定义即可 14 | cd bluesea2 //进入该文件夹 15 | git clone https://github.com/BlueSeaLidar/bluesea-ros2.git src //下载驱动包并且重命名为src 16 | 17 | 2.构建 18 | 19 | colcon build 20 | 3.更新当前ROS2包环境 21 | 22 | source ./install/setup.sh 23 | 24 | 25 | 4.使用ROS2 launch运行驱动 26 | 27 | sudo chmod 777 /dev/ttyUSB0 (非网络款) //这里的/dev/ttyUSB0指的是串口名称，如果是串口/虚拟串口款，需要赋权 28 | 29 | ros2 launch bluesea2 [launch file] //具体launch文件说明如下 30 | 31 | ## 驱动包launch配置文件说明 32 | 说明：[launch file]指的是src/launch文件夹下的配置文件，以功能类别区分 33 | 34 | - uart_lidar.launch: 串口连接方式的激光雷达 35 | - udp_lidar.launch: udp网络通讯的激光雷达 36 | - vpc_lidar.launch：虚拟串口连接方式的激光雷达 37 | - dual_udp_lidar.launch：多个udp网络通讯的激光雷达 38 | - template.launch：全部参数定义模版 39 | - LDS-U50C-S(only).launch:老款雷达，仅支持数据通讯 40 | - LDS-U80C-S(only).launch:老款雷达，仅支持数据通讯 41 | - 42 | 主要参数配置说明： 43 | 44 | #ROS2#（框架必须的参数） 45 | #发布scan话题 46 | #发布cloud话题 47 | #标志坐标系的名称 48 | #DATA#（驱动自定义数据层面的限制参数） 49 | #最小点云距离(m) 50 | #最大点云距离(m) 51 | #帧起始角度是否从0开始(false 为180) 52 | #二维扫描数据(默认) 53 | #三维空间数据 54 | #三维空间数据输出格式2 55 | #按帧输出 56 | #发布数据角度参数反置(angle_min，angle_max,angle_increment) 57 | #发布数据点云数据倒转(从最后一个点排到第一个点) 58 | #硬采样系数(前提需要雷达支持该指令) 59 | #软采样系数(前提需要点云大于软采样的最小点数) 60 | #角度过滤开关 61 | #最小可用角度 62 | [-3.14,3.14]#屏蔽该区间角度的数据 64 | #数据包的时间戳来源(默认0系统时间，1是雷达时间同步的时间) 65 | #多段屏蔽该区间角度的数据，mask往上+1 66 | #CUSTOM#（驱动自定义功能） 67 | #判断距离为0的点的比重连续三圈 68 | #每圈距离为0的点占据90% 则报错 69 | #仅用于监听组播数据(需要上位机修改雷达的上传地址,并且固定上传，然后该驱动监听数据) 70 | #监听的组播ip 71 | #FITTER#(雷达不同角分辨率的参数不同，需定制) 72 | #滤波生效开关 73 | #滤波生效的最小距离 75 | #离异点判断的物理范围 76 | #判断离异点下标的范围 77 | #CONNECT#（驱动连接雷达的参数） 78 | 79 | 80 | 81 | 82 | #GET#（驱动发送给雷达的查询指令开关） 83 | #查询雷达SN号，-1不查询，>=0查询 84 | #SET#（驱动发送给雷达的设置指令开关） 85 | #设置角分辨率（不同型号雷达可支持的角分辨率不相同，具体查看该型号的说明书）,-1不设置 0原始数据 1角度修正 87 | #设置去拖点,-1不设置 0关闭 1打开 88 | #设置滤波,-1不设置 0关闭 1打开 89 | #设置报警信息,-1不设置 0关闭 1打开 90 | #设置旋转方向(仅支持该指令的雷达使用),-1不设置 0关闭 1打开 91 | #NTP# 92 | # NTP 服务器地址 93 | #NTP 服务器端口 94 | #使能开关 95 | ## 驱动客户端功能说明 96 | 源码位于src/client.cpp 97 | 启停旋转： 98 | 99 | ros2 run bluesea2 bluesea2_client scan start 100 | 参数1 是话题参数2是动作(start/stop) 101 | 切换防区： 102 | 103 | ros2 run bluesea2 bluesea2_client scan switchZone 0 104 | 参数1 是话题参数2是动作(switchZone) 参数3是选择防区编号(0) 105 | 106 | 107 | 设置转速： 108 | 109 | ros2 run bluesea2 bluesea2_client scan rpm 600 110 | 参数1 是话题参数2是动作(set rpm) 参数3是设置rpm的值(600) 111 | 112 | 打开/关闭心跳包检测： 113 | 114 | ros2 run bluesea2 bluesea2_client heart check 1 115 | 参数1是service name 参数2是动作(check) 参数3是是否打印(1/0) 116 | 117 | 118 | ## rosbag包操作说明 119 | 120 | ros2 topic list 获取话题列表，驱动默认的话题名称为 /lidar1/scan 121 | ros2 bag record /lidar1/scan 开始录制数据 122 | 123 | 录制好的文件以时间戳来命名，要停止录制，在当前终端CTRL+C 124 | 125 | ros2 bag play 数据包名称 126 | 127 | 在存放数据包的路径下查看录制的数据包，若提示failed connect master异常，则先ros master后在rosbag play 128 | 129 | ## 商务支持 130 | 131 | 具体使用问题请通过官网联系技术支持(https://pacecat.com/) 132 | -------------------------------------------------------------------------------- /bluesea-ros2/sdk/include/usererror.h: -------------------------------------------------------------------------------- 1 | #ifndef _USERERROR_H 2 | #define _USERERROR_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #define UNUSED(x) (void)(x) 9 | 10 | class Error 11 | { 12 | public: 13 | Error(int value, const std::string& str) 14 | { 15 | m_value = value; 16 | m_message = str; 17 | #ifdef _DEBUG 18 | ErrorMap::iterator found = GetErrorMap().find(value); 19 | if (found != GetErrorMap().end()) 20 | assert(found->second == m_message); 21 | #endif 22 | GetErrorMap()[m_value] = m_message; 23 | } 24 | 25 | // auto-cast Error to integer error code 26 | operator int() { return m_value; } 27 | 28 | private: 29 | int m_value; 30 | std::string m_message; 31 | 32 | typedef std::map ErrorMap; 33 | static ErrorMap& GetErrorMap() 34 | { 35 | static ErrorMap errMap; 36 | return errMap; 37 | } 38 | 39 | public: 40 | 41 | static std::string GetErrorString(int value) 42 | { 43 | ErrorMap::iterator found = GetErrorMap().find(value); 44 | if (found == GetErrorMap().end()) 45 | { 46 | assert(false); 47 | return ""; 48 | } 49 | else 50 | { 51 | return found->second; 52 | } 53 | } 54 | }; 55 | 56 | static Error CMD_NO_ANSWER(-101,"send commands to lidar but no answer,please check the communication connection or retry"); 57 | static Error SOCKET_ERR(-102,"lidar connection abnormality, please check the hardware plugging or communication situation"); 58 | static Error LIDAR_NUM_LARGE(-103,"The number of lidars added exceeds the maximum limit"); 59 | static Error PORT_BIND_ERR(-104,"Port binding failed, occupied"); 60 | static Error COM_OPEN_ERR(-105,"Failed to open serial port, please check if it is occupied"); 61 | static Error CHANGE_BAUDRATE_ERR(-106,"Failure to modify serial port baud rate"); 62 | static Error GET_COM_ARG_ERR(-107,"Failure to get serial port parameters"); 63 | static Error SET_COM_ARG_ERR(-108,"Failure to modify serial port parameters"); 64 | static Error COM_READ_ERR(-109,"Failed to read data through serial port,please try after re-plugging the device"); 65 | static Error POINT_NUM_LARGE(-110,"The number of point clouds exceeds the maximum limit, please contact the developer"); 66 | static Error MEMORY_SPACE_ERR(-111,"Program application memory space is insufficient, please contact the developer"); 67 | static Error SAMPLE_TOO_SMALL(-112,"Soft Sampling Angle resolution too small, please reset"); 68 | static Error DISTANCE_ZERO_LARGE(-113,"Points with a distance of zero scale beyond the error_scale factor of the configuration file"); 69 | static Error DEVICE_OFFLINE(-114,"Serial device list does not exist, please check the device connection"); 70 | static Error CHECKSUM_ERR(-115,"Sector data calibration error, please check whether the data line is in accordance with the specification"); 71 | static Error TOPIC_NO_FIND(-116,"Find a topic that does not exist"); 72 | static Error NTP_IP_FORMAT_ERROR(-117,"ntp ip format set error"); 73 | static Error DROP_PACKET(-118,"drop packet"); 74 | static Error SUBPACKET_MERGE_ERROR(-119,"subpacket merge timestamp error"); 75 | static Error SUBPACKET_REPEAT(-120,"subpacket data repeat"); 76 | 77 | 78 | static Error ALARM_LOWPOWER(-201,"Insufficient power supply, or occasional undervoltage"); 79 | static Error ALARM_ZERO_ERR(-202,"EnCoding disk zero detection error, please contact the developer"); 80 | static Error ALARM_CODEDISK_ERR(-203,"EnCoding disk error, please check if the radar is shaking badly or contact the developer"); 81 | static Error ALARM_RESET_ERR(-204,"Voltage reset failed, please try powering up again"); 82 | static Error ALARM_MOUDLE_ERR(-205,"Insufficient voltage or abnormal temperature for a long period of time"); 83 | 84 | static Error ALARM_BOTTOMPLATE_LOW_VOLTAGE(-206,"Bottom plate low voltage,please check the power supply or contact the developer"); 85 | static Error ALARM_BOTTOMPLATE_HIGH_VOLTAGE(-207,"Bottom plate high voltage,please check the power supply or contact the developer"); 86 | static Error ALARM_TEMPERATURE_ERR(-208,"Abnormal motor head temperature,please pay attention to the heat dissipation of the current working environment"); 87 | static Error ALARM_MOTERHEAD_LOW_VOLTAGE(-209,"Motor head low voltage,please check the power supply or contact the developer"); 88 | static Error ALARM_MOTERHEAD_HIGH_VOLTAGE(-210,"Motor head high voltage,please check the power supply or contact the developer"); 89 | //ERROR((int)LIDAR_NUM_LARGE,Error::GetErrorString(LIDAR_NUM_LARGE).c_str()); 90 | 91 | //log日志 92 | #define __DEBUG__ 93 | 94 | #ifdef __DEBUG__ 95 | #define DEBUG(format,...) \ 96 | {timespec spec;\ 97 | clock_gettime(CLOCK_REALTIME,&spec);\ 98 | printf("[INFO] [%ld.%09ld] [%s %d]: " format "\n",spec.tv_sec,spec.tv_nsec,__FUNCTION__,__LINE__,##__VA_ARGS__);} 99 | 100 | #define ERROR(code,value) \ 101 | {timespec spec;\ 102 | clock_gettime(CLOCK_REALTIME,&spec);\ 103 | printf("\033[0m\033[1;31m[ERROR] [%ld.%09ld] [%s %d]: code:%d value:%s\033[0m\n",spec.tv_sec,spec.tv_nsec,__FUNCTION__,__LINE__,(int)code,(char*)value);} 104 | 105 | #else 106 | #define DEBUG(format,...) 107 | #endif 108 | 109 | 110 | #endif // USERERROR_ 111 | -------------------------------------------------------------------------------- /bluesea-ros2/src/heart_check.cpp: -------------------------------------------------------------------------------- 1 | #include "rclcpp/clock.hpp" 2 | #include "rclcpp/rclcpp.hpp" 3 | #include "rclcpp/time_source.hpp" 4 | #include "sensor_msgs/msg/laser_scan.hpp" 5 | #include 6 | #include 7 | #include "../sdk/include/reader.h" 8 | #include "base/srv/control.hpp" 9 | 10 | bool g_isrun = false; 11 | pthread_t g_threadid; 12 | void getTime_HMS(char *data); 13 | void *HeartThreadProc(void*p); 14 | 15 | bool heart_motor(const std::shared_ptr req, std::shared_ptr res) 16 | { 17 | UNUSED(res); 18 | DEBUG("HeartCheck change status:%d\n:", req->flag); 19 | if(req->flag==g_isrun) 20 | return true; 21 | else 22 | { 23 | if(req->flag) 24 | pthread_create(&g_threadid, NULL, HeartThreadProc, NULL); 25 | 26 | g_isrun = req->flag; 27 | } 28 | return true; 29 | } 30 | using std::placeholders::_1; 31 | using std::placeholders::_2; 32 | int main(int argc, char **argv) 33 | { 34 | rclcpp::init(argc, argv); 35 | std::shared_ptr node = rclcpp::Node::make_shared("bluesea_node"); 36 | rclcpp::QoS qos(0); 37 | qos.reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE); 38 | qos.durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); 39 | 40 | rclcpp::Service::SharedPtr service = node->create_service("/heart_motor", heart_motor); 41 | 42 | DEBUG("bluesea2_heart_check into\n"); 43 | rclcpp::spin(node); 44 | return 0; 45 | } 46 | void getTime_HMS(char *data) 47 | { 48 | time_t t0 = time(NULL); 49 | int hh = t0 % (3600 * 24) / 3600; 50 | int mm = t0 % 3600 / 60; 51 | int ss = t0 % 60; 52 | sprintf(data, "%d-%d-%d", hh, mm, ss); 53 | } 54 | // 打印具体信息 55 | void HeartMsg(char *buf_data, int buf_len) 56 | { 57 | 58 | if (buf_len == sizeof(DevInfoV101)) 59 | { 60 | DevInfoV101 *dvi = (DevInfoV101 *)buf_data; 61 | DEBUG("timestamp:%u.%u sn:%s model:%s id:%d rpm:%.1f", dvi->timestamp[0], dvi->timestamp[1], dvi->dev_sn, dvi->dev_type, dvi->dev_id, dvi->rpm / 10.0); 62 | char tmp_IPv4[16] = {0}; 63 | char tmp_mask[16] = {0}; 64 | char tmp_gateway[16] = {0}; 65 | char tmp_srv_ip[16] = {0}; 66 | 67 | sprintf(tmp_IPv4, "%d.%d.%d.%d", dvi->ip[0], dvi->ip[1], dvi->ip[2], dvi->ip[3]); 68 | sprintf(tmp_mask, "%d.%d.%d.%d", dvi->mask[0], dvi->mask[1], dvi->mask[2], dvi->mask[3]); 69 | sprintf(tmp_gateway, "%d.%d.%d.%d", dvi->gateway[0], dvi->gateway[1], dvi->gateway[2], dvi->gateway[3]); 70 | sprintf(tmp_srv_ip, "%d.%d.%d.%d", dvi->remote_ip[0], dvi->remote_ip[1], dvi->remote_ip[2], dvi->remote_ip[3]); 71 | DEBUG("lidar_ip:%s lidar_mask:%s lidar_gateway:%s lidar_port:%d upload_ip:%s upload_port:%d", tmp_IPv4, tmp_mask, tmp_gateway, dvi->port, tmp_srv_ip, dvi->remote_udp); 72 | std::string alarmMsg; 73 | if (dvi->alarm[FUN_LV_1]) 74 | alarmMsg += " Observation Zone"; 75 | if (dvi->alarm[FUN_LV_2]) 76 | alarmMsg += " Alert Zone"; 77 | if (dvi->alarm[FUN_LV_3]) 78 | alarmMsg += " Alarm Zone"; 79 | if (dvi->alarm[FUN_COVER]) 80 | alarmMsg += " Masking"; 81 | if (dvi->alarm[FUN_NO_DATA]) 82 | alarmMsg += " No data"; 83 | if (!dvi->alarm[FUN_ZONE_ACTIVE]) 84 | alarmMsg += " No zone active"; 85 | if (dvi->alarm[FUN_SYS_ERR]) 86 | alarmMsg += " System error"; 87 | int v = dvi->status & 0x7; 88 | if (v == 1 || v == 3) 89 | { 90 | if (!dvi->alarm[FUN_NET_LINK]) 91 | alarmMsg += " NET disconnected"; 92 | } 93 | else if (v == 2) 94 | { 95 | if (!dvi->alarm[FUN_USB_LINK]) 96 | alarmMsg += " USB disconnected"; 97 | } 98 | if (dvi->alarm[FUN_UPDATING]) 99 | alarmMsg += " System Upgrade"; 100 | if (dvi->alarm[FUN_ZONE_DEFINED]) 101 | alarmMsg += " Zone not defined"; 102 | 103 | if(!alarmMsg.empty()) 104 | DEBUG("lidar_alarm:%s",alarmMsg.c_str()); 105 | } 106 | 107 | } 108 | 109 | void *HeartThreadProc(void*p) 110 | { 111 | UNUSED(p); 112 | int sock = socket(AF_INET, SOCK_DGRAM, 0); 113 | int yes = 1; 114 | if (setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, (char *)&yes, sizeof(yes)) < 0) 115 | { 116 | return NULL; 117 | } 118 | sockaddr_in addr; 119 | addr.sin_family = AF_INET; 120 | addr.sin_port = htons(HeartPort); 121 | addr.sin_addr.s_addr = htonl(INADDR_ANY); 122 | 123 | int iResult = ::bind(sock, (struct sockaddr *)&addr, sizeof(addr)); 124 | if (iResult != 0) 125 | return NULL; 126 | 127 | struct ip_mreq mreq; 128 | mreq.imr_multiaddr.s_addr = inet_addr("225.225.225.225"); 129 | mreq.imr_interface.s_addr = htonl(INADDR_ANY); 130 | if (setsockopt(sock, IPPROTO_IP, IP_ADD_MEMBERSHIP, (char *)&mreq, sizeof(mreq)) < 0) 131 | { 132 | return NULL; 133 | } 134 | while (g_isrun) 135 | { 136 | socklen_t sz = sizeof(addr); 137 | char raw[4096]; 138 | int dw = recvfrom(sock, raw, sizeof(raw), 0, (struct sockaddr *)&addr, &sz); 139 | if (memcmp(raw, "LiDA", 4) == 0) 140 | { 141 | HeartMsg(raw, dw); 142 | } 143 | } 144 | close(sock); 145 | return NULL; 146 | } -------------------------------------------------------------------------------- /bluesea-ros2/rviz/demo.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /LaserScan1 10 | - /LaserScan1/Topic1 11 | Splitter Ratio: 0.5 12 | Tree Height: 253 13 | - Class: rviz_common/Selection 14 | Name: Selection 15 | - Class: rviz_common/Tool Properties 16 | Expanded: 17 | - /2D Goal Pose1 18 | - /Publish Point1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.5886790156364441 21 | - Class: rviz_common/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.5 26 | - Class: rviz_common/Time 27 | Experimental: false 28 | Name: Time 29 | SyncMode: 0 30 | SyncSource: LaserScan 31 | Visualization Manager: 32 | Class: "" 33 | Displays: 34 | - Alpha: 0.5 35 | Cell Size: 1 36 | Class: rviz_default_plugins/Grid 37 | Color: 160; 160; 164 38 | Enabled: true 39 | Line Style: 40 | Line Width: 0.029999999329447746 41 | Value: Lines 42 | Name: Grid 43 | Normal Cell Count: 0 44 | Offset: 45 | X: 0 46 | Y: 0 47 | Z: 0 48 | Plane: XY 49 | Plane Cell Count: 10 50 | Reference Frame: 51 | Value: true 52 | - Alpha: 1 53 | Autocompute Intensity Bounds: true 54 | Autocompute Value Bounds: 55 | Max Value: 10 56 | Min Value: -10 57 | Value: true 58 | Axis: Z 59 | Channel Name: intensity 60 | Class: rviz_default_plugins/LaserScan 61 | Color: 255; 255; 255 62 | Color Transformer: Intensity 63 | Decay Time: 0 64 | Enabled: true 65 | Invert Rainbow: false 66 | Max Color: 255; 255; 255 67 | Max Intensity: 37 68 | Min Color: 0; 0; 0 69 | Min Intensity: 19 70 | Name: LaserScan 71 | Position Transformer: XYZ 72 | Selectable: true 73 | Size (Pixels): 3 74 | Size (m): 0.009999999776482582 75 | Style: Flat Squares 76 | Topic: 77 | Depth: 5 78 | Durability Policy: Volatile 79 | Filter size: 10 80 | History Policy: Keep Last 81 | Reliability Policy: Best Effort 82 | Value: /scan 83 | Use Fixed Frame: true 84 | Use rainbow: true 85 | Value: true 86 | Enabled: true 87 | Global Options: 88 | Background Color: 48; 48; 48 89 | Fixed Frame: map 90 | Frame Rate: 30 91 | Name: root 92 | Tools: 93 | - Class: rviz_default_plugins/Interact 94 | Hide Inactive Objects: true 95 | - Class: rviz_default_plugins/MoveCamera 96 | - Class: rviz_default_plugins/Select 97 | - Class: rviz_default_plugins/FocusCamera 98 | - Class: rviz_default_plugins/Measure 99 | Line color: 128; 128; 0 100 | - Class: rviz_default_plugins/SetInitialPose 101 | Covariance x: 0.25 102 | Covariance y: 0.25 103 | Covariance yaw: 0.06853891909122467 104 | Topic: 105 | Depth: 5 106 | Durability Policy: Volatile 107 | History Policy: Keep Last 108 | Reliability Policy: Reliable 109 | Value: /initialpose 110 | - Class: rviz_default_plugins/SetGoal 111 | Topic: 112 | Depth: 5 113 | Durability Policy: Volatile 114 | History Policy: Keep Last 115 | Reliability Policy: Reliable 116 | Value: /goal_pose 117 | - Class: rviz_default_plugins/PublishPoint 118 | Single click: true 119 | Topic: 120 | Depth: 5 121 | Durability Policy: Volatile 122 | History Policy: Keep Last 123 | Reliability Policy: Reliable 124 | Value: /clicked_point 125 | Transformation: 126 | Current: 127 | Class: rviz_default_plugins/TF 128 | Value: true 129 | Views: 130 | Current: 131 | Class: rviz_default_plugins/Orbit 132 | Distance: 3.1647839546203613 133 | Enable Stereo Rendering: 134 | Stereo Eye Separation: 0.05999999865889549 135 | Stereo Focal Distance: 1 136 | Swap Stereo Eyes: false 137 | Value: false 138 | Focal Point: 139 | X: 0 140 | Y: 0 141 | Z: 0 142 | Focal Shape Fixed Size: true 143 | Focal Shape Size: 0.05000000074505806 144 | Invert Z Axis: false 145 | Name: Current View 146 | Near Clip Distance: 0.009999999776482582 147 | Pitch: 0.785398006439209 148 | Target Frame: 149 | Value: Orbit (rviz) 150 | Yaw: 0.785398006439209 151 | Saved: ~ 152 | Window Geometry: 153 | Displays: 154 | collapsed: false 155 | Height: 550 156 | Hide Left Dock: false 157 | Hide Right Dock: false 158 | QMainWindow State: 000000ff00000000fd00000004000000000000015600000188fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001c300000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000188000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000188fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000188000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000050c0000003efc0100000002fb0000000800540069006d006501000000000000050c000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000029b0000018800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 159 | Selection: 160 | collapsed: false 161 | Time: 162 | collapsed: false 163 | Tool Properties: 164 | collapsed: false 165 | Views: 166 | collapsed: false 167 | Width: 1292 168 | X: 72 169 | Y: 27 170 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # BLUESEA ROS2 driver V2.6 2 | 3 | ## Overview 4 | ---------- 5 | BLUESEA ROS2 driver is specially designed to connect to the lidar products produced by our company. The driver can run on operating systems with ROS2 installed, and mainly supports ubuntu series operating systems (18.04LTS-now). The hardware platforms that have been tested to run the ROS2 driver include: Intel x86 mainstream CPU platform, and some ARM64 hardware platforms (such as NVIDIA, Rockchip, Raspberry Pi, etc., which may need to update the cp210x driver). 6 | 7 | ## Note 8 | ---------- 9 | Please ensure that the path does not contain Chinese characters, otherwise the compilation will fail! 10 | ## Get and build the BLUESEA ROS driver package 11 | 1.Get the BLUESEA ROS driver from Github and deploy the corresponding location 12 | 13 | mkdir bluesea2 //create a folder and customize it 14 | cd bluesea2 //into this folder 15 | git clone https://github.com/BlueSeaLidar/bluesea-ros2.git src //download the driver package and rename it to src 16 | 2.Build 17 | 18 | colcon build 19 | 3.Update the current ROS2 package environment 20 | 21 | source ./install/setup.sh 22 | 23 | 24 | 4.Using ROS2 launch to run drivers 25 | 26 | sudo chmod 777 /dev/ttyUSB0 (uart) // /dev/ttyUSB0 refers to the serial port name. If it is a serial/virtual serial port model, it needs to be authorized 27 | 28 | ros2 launch bluesea2 [launch file] //The specific launch file description is as follows 29 | 30 | ## Driver launch launch file 31 | explain：[launch file] refers to the configuration files in the src/launch folder, distinguished by functional categories 32 | 33 | - uart_lidar.launch(clockwise/anticlockwise): lidar with serial port connection method 34 | - udp_lidar.launch(clockwise/anticlockwise): lidar for UDP network communication 35 | - vpc_lidar.launch(clockwise/anticlockwise)： lidar with virtual serial port connection method 36 | - dual_udp_lidar.launch(clockwise/anticlockwise)： lidar with multiple UDP network communication(only one node) 37 | - template： All parameter definition templates 38 | - LDS-U50C-S(only).launch:Older lidars, data communication only 39 | - LDS-U80C-S(only).launch:Older lidars, data communication only 40 | 41 | Main parameter configuration instructions： 42 | 43 | #ROS2# (mandatory parameter for the framework) 44 | #Publish topic scan 45 | #Publish topic cloud 46 | #Name of the flag coordinate system 47 | #DATA# (driver-defined data level limiting parameter) 48 | #Minimum point cloud distance (m) 49 | #Maximum point cloud distance (m) 50 | #Whether start angle is from 0 (false is 180). 51 | #2D scan data (default) 52 | #3D spatial data. 53 | #三维空间数据输出格式2 54 | #Output by frame. 55 | #Publish data angle parameter inverted(angle_min,angle_max,angle_increment). 56 | #Publish data point cloud data reversed (row from last point to first point) 57 | #hard_resample_factor(if lidar support this command) 58 | #Soft resample coefficient (need point cloud larger than the minimum number of points for soft resample) 59 | #Angle filter switch. 60 | #Minimum available angle. 61 | [-3.14,3.14]#mask data for angle in this interval 63 | #Timestamp source of packet (default 0 system time, 1 is lidar time synchronized time) 64 | [-1,0]#Multiple segments to mask the angle of the interval, mask upwards +1 65 | #CUSTOM# (driver customization function) 66 | # Judge the weight of the point with distance 0 Three consecutive circles. 67 | # 90% of points with distance 0 in each circle will report error. 68 | #Only used to listen to multicast data (need to change the upload address of lidar by host computer, and fix the upload, then this driver will listen to the data). 69 | #Listen to multicast ip. 70 | #FITTER# (lidar different angular resolution parameters are different, need to be customized) 71 | #Filter enable switch. 72 | #Minimum range for filtering. 74 | #Physical range to judge the divergence. 75 | #Range of subscripts for judging outliers 76 | #CONNECT# (parameter that drives the connection lidar) 77 | 78 | 79 | 80 | 81 | #GET# (the query command switch that the driver sends to the lidar) 82 | #Query lidar SN number, -1 no query, >=0 query 83 | #SET# (set command switch that driver sends to lidar) 84 | #set angular resolution (different lidar models can support different angular resolution, check the manual of the model),-1 not set 0 original data 1 angular correction 86 | #Set de-smooth point, -1 not set 0 off 1 on. 87 | #Set filtering, -1 not set 0 off 1 on 88 | #set alarm message, -1 not set 0 off 1 on 89 | #Set direction of rotation(only used by lidar which support this command),-1 not set 0 off 1 on 90 | #NTP# 91 | # NTP service ip 92 | #NTP service port 93 | # enable or not 94 | 95 | ## Driver Client Functional Description 96 | source code locate at src/client.cpp 97 | start/stop rotate： 98 | 99 | ros2 run bluesea2 bluesea_node_client scan start 100 | arg1 is topic arg2 is action(start/stop) 101 | 102 | switch defense zones： 103 | 104 | ros2 run bluesea2 bluesea2_client scan switchZone 0 105 | arg1 is topic arg2 is action(switchZone) arg3 is select zone id(0) 106 | 107 | set rpm： 108 | 109 | ros2 run bluesea2 bluesea2_client scan rpm 600 110 | arg1 is topic arg2 is action(set rpm) arg3 is rpm value(600) 111 | 112 | set heart check: 113 | 114 | ros2 run bluesea2 bluesea2_client heart check 1 115 | arg1 is service name arg2 is action(check) arg3 is print(1) / not print(0) 116 | 117 | ## rosbag bag operating instructions 118 | 119 | ros2 topic list 120 | Get the topic list, the driver default topic name is /lidar1/scan 121 | 122 | ros2 bag record /lidar1/scan 123 | 124 | Start recording data. 125 | 126 | The recorded file is named with a timestamp, to stop recording, CTRL+C in the current terminal 127 | 128 | ros2 bag play packet name 129 | 130 | Check the recorded packet in the path where the packet is stored, if it prompts failed connect master exception, then ros master first and then rosbag play. 131 | 132 | ## Business Support 133 | 134 | Please contact the technical support (https://pacecat.com/) through the official website for specific usage problems. 135 | -------------------------------------------------------------------------------- /bluesea-ros2/sdk/include/common_data.h: -------------------------------------------------------------------------------- 1 | #ifndef _COMMON_DATA_ 2 | #define _COMMON_DATA_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #define DF_UNIT_IS_MM 0x0001 10 | #define DF_WITH_INTENSITY 0X0002 11 | #define DF_DESHADOWED 0x0004 12 | #define DF_SMOOTHED 0x0008 13 | #define DF_FAN_90 0x0020 14 | #define DF_WITH_RPM 0X0040 15 | #define DF_WITH_RESAMPLE 0X0010 16 | #define DF_WITH_RESAMPLE_SOFT 0X0080 17 | #define DF_MOTOR_REVERSE 0x0100 18 | #define DF_WITH_UUID 0X1000 19 | 20 | #define EF_ENABLE_ALARM_MSG 0X10000 21 | 22 | #define MAX_FANS 120 23 | #define MAX_POINTS 510 24 | #define ANYONE -1 25 | #define HDR_SIZE 6 26 | #define HDR2_SIZE 8 27 | #define HDR3_SIZE 16 28 | #define HDR7_SIZE 28 29 | #define HDR99_SIZE 32 30 | #define HDRAA_SIZE 48 31 | #define BUF_SIZE 8 * 1024 32 | #define MAX_LIDARS 8 33 | #define GS_PACK 0x4753 34 | #define S_PACK 0x0053 35 | #define C_PACK 0x0043 36 | #ifndef M_PI 37 | #define M_PI 3.1415926535898 38 | #endif 39 | 40 | #define getbit(x, y) ((x) >> (y)&1) // 获取X的Y位置数据 41 | #define setbit(x, y) x |= (1 << y) // 将X的第Y位置1 42 | #define clrbit(x, y) x &= ~(1 << y) // 将X的第Y位清0 43 | 44 | #define HeartPort 6789 45 | struct DataPoint 46 | { 47 | //uint16_t idx; 48 | // int angle; 49 | double degree; 50 | uint32_t distance; // mm 51 | uint8_t confidence; 52 | }; 53 | struct CmdHeader 54 | { 55 | unsigned short sign; 56 | unsigned short cmd; 57 | unsigned short sn; 58 | unsigned short len; 59 | }; 60 | 61 | struct RawData 62 | { 63 | unsigned short code; 64 | unsigned short N; 65 | unsigned short angle; // 0.1 degree 66 | unsigned short span; // 0.1 degree 67 | unsigned short fbase; 68 | unsigned short first; 69 | unsigned short last; 70 | unsigned short fend; 71 | // short ros_angle; // 0.1 degree 72 | DataPoint points[MAX_POINTS]; 73 | uint32_t ts[2]; 74 | int8_t counterclockwise; 75 | uint32_t flags; //消息类型 76 | }; 77 | struct AllPointData 78 | { 79 | std::vectorpoints;//已缓存的所有数据 80 | std::vectortimestamp;//时间戳[2] 81 | 82 | }; 83 | 84 | struct EEpromV101 85 | { 86 | char label[4]; // "EPRM" 87 | uint16_t pp_ver; // 协议版本 88 | uint16_t size; // 结构大小 89 | 90 | //uint32_t version; // firmware version 91 | 92 | // device 93 | uint8_t dev_sn[20];//sn号 94 | uint8_t dev_type[16];//型号 95 | uint32_t dev_id; // 编号 96 | 97 | // network 98 | uint8_t IPv4[4]; 99 | uint8_t mask[4]; 100 | uint8_t gateway[4]; 101 | uint8_t srv_ip[4];//上传IP 102 | uint16_t srv_port;//上传端口 103 | uint16_t local_port;//本地端口 104 | 105 | // 106 | uint16_t RPM; 107 | uint16_t zone_dist_thr;/* 防区连续检测距离阈值 */ 108 | uint8_t fir_filter;//防区报警点数标准 109 | uint8_t cir;//防区报警圈数过滤标准 110 | uint16_t with_resample; 111 | 112 | uint8_t auto_start;//开机自动旋转 113 | uint8_t target_fixed;//固定上传 114 | uint8_t with_smooth;//数据平滑 115 | uint8_t with_filter;//去拖点 116 | 117 | 118 | int16_t dist_comp_coeff_a; 119 | int16_t dist_comp_coeff_b; 120 | uint16_t dist_comp_range_1; 121 | uint16_t dist_comp_range_2; 122 | uint32_t net_watchdog;//看门狗 123 | 124 | uint32_t pnp_flags;//PNP/NPN 125 | uint16_t deshadow;//平滑系数 126 | uint8_t zone_acted;//激活防区 127 | uint8_t should_post;//上传方式 0 无数据 1仅数据 2报警 3报警+数据 128 | 129 | uint8_t functions_map[16];//I/O输入输出口 130 | int16_t zero_pos; //零位校正 131 | uint16_t wlp_magic; // 无线 132 | uint16_t wlp_pwm; // 无线供电 133 | uint8_t motor_ccw; // 旋转方向 134 | uint8_t reserved[29]; 135 | 136 | }; 137 | 138 | 139 | 140 | typedef void *HReader; 141 | typedef void *HPublish; 142 | 143 | 144 | struct RawDataHdr 145 | { 146 | unsigned short code; 147 | unsigned short N; 148 | unsigned short angle; 149 | }; 150 | 151 | struct RawDataHdr2 152 | { 153 | unsigned short code; 154 | unsigned short N; 155 | unsigned short angle; 156 | unsigned short span; 157 | }; 158 | 159 | struct RawDataHdr3 160 | { 161 | unsigned short code; 162 | unsigned short N; 163 | unsigned short angle; 164 | unsigned short span; 165 | unsigned short fbase; 166 | unsigned short first; 167 | unsigned short last; 168 | unsigned short fend; 169 | }; 170 | 171 | struct RawDataHdr7 172 | { 173 | uint16_t code; 174 | uint16_t N; 175 | uint16_t whole_fan; 176 | uint16_t ofset; 177 | uint32_t beg_ang; 178 | uint32_t end_ang; 179 | uint32_t flags; 180 | uint32_t timestamp; 181 | uint32_t dev_id; 182 | }; 183 | struct RawDataHdrAA { 184 | uint16_t code; // 0xFAAA 185 | uint16_t N; 186 | uint16_t whole_fan; 187 | uint16_t ofset; 188 | uint32_t beg_ang; 189 | uint32_t end_ang; 190 | uint32_t flags; 191 | uint32_t second; 192 | uint32_t nano_sec; 193 | uint32_t dev_id; 194 | uint32_t reserved[4]; 195 | }; 196 | struct RawDataHdr99 197 | { 198 | uint16_t code; 199 | uint16_t N; 200 | uint16_t from; 201 | uint16_t total; 202 | uint32_t flags; 203 | uint32_t timestamp; 204 | uint32_t dev_no; 205 | uint32_t reserved[3]; 206 | }; 207 | 208 | typedef struct 209 | { 210 | uint16_t code; 211 | uint16_t len; 212 | uint32_t clk; 213 | uint32_t dev_id; 214 | } PacketUart; 215 | 216 | struct FanSegment_C7 217 | { 218 | RawDataHdr7 hdr; 219 | 220 | uint16_t dist[MAX_POINTS]; 221 | uint16_t angle[MAX_POINTS]; 222 | uint8_t energy[MAX_POINTS]; 223 | 224 | struct FanSegment_C7 *next; 225 | }; 226 | struct FanSegment_AA 227 | { 228 | RawDataHdrAA hdr; //CN：HdrAA结构体 EN：Hdr7structure 229 | 230 | uint16_t dist[MAX_POINTS]; //CN:距离 EN:distance 231 | uint16_t angle[MAX_POINTS]; //CN:角度 EN:angle 232 | uint8_t energy[MAX_POINTS]; //CN:能量强度 EN:energy intensity 233 | 234 | struct FanSegment_AA* next; // CN:下个扇区指针 EN:next sector pointer 235 | }; 236 | struct CommandList 237 | { 238 | char uuid[12]; 239 | char model[12]; 240 | char rpm[12]; 241 | char res[12]; 242 | char smooth[12]; 243 | char fitter[12]; 244 | char confidence[12]; 245 | char unit_mm[12]; 246 | 247 | char alarm[12]; 248 | char direction[12]; 249 | char ats[12]; 250 | char ntp[64]; 251 | 252 | 253 | }; 254 | struct Parser 255 | { 256 | int rest_len; 257 | unsigned char rest_buf[BUF_SIZE]; 258 | bool with_chk; 259 | int raw_mode; 260 | uint32_t dev_id; 261 | uint32_t flags; 262 | FanSegment_AA *fan_segs_aa; 263 | FanSegment_C7 *fan_segs_c7; 264 | int error_circle; 265 | float error_scale; 266 | bool is_from_zero; 267 | int time_mode; 268 | CommandList cmd; 269 | char ip[32]; 270 | int port; 271 | bool direction; 272 | bool isrun; 273 | }; 274 | struct PubHub 275 | { 276 | pthread_mutex_t mtx; 277 | int nfan; 278 | RawData *fans[MAX_FANS]; 279 | int error_num; 280 | int offsetangle; 281 | int offsetidx; 282 | std::vectorconsume;//统计完剩余的点数 283 | uint32_t ts_beg[2]; 284 | uint32_t ts_end[2]; 285 | }; 286 | 287 | struct LidarNode 288 | { 289 | Parser* hParser; 290 | PubHub* hPublish; 291 | char ip[16]; 292 | int port; 293 | bool hasdata; 294 | uint32_t nodata_count; 295 | }; 296 | struct UartState 297 | { 298 | //byte1 299 | bool unit_mm;//0 cm 1 mm 300 | bool with_conf;//0 close 1 open 301 | bool with_smooth; 302 | bool with_fitter; 303 | bool span_9; 304 | bool span_18; 305 | bool span_other; 306 | bool resampele;//重采样 307 | //byte2 308 | 309 | bool moter_turn;//0正向 1反向 310 | bool span_8; 311 | bool span_16; 312 | //byte3 313 | bool byte3_error0; 314 | bool byte3_error1; 315 | bool byte3_error2; 316 | bool byte3_error3; 317 | //byte4 318 | bool byte4_error0; 319 | bool byte4_error1; 320 | bool byte4_error2; 321 | bool byte4_error3; 322 | bool byte4_error4; 323 | bool byte4_error5; 324 | bool byte4_error6; 325 | bool byte4_error7; 326 | }; 327 | typedef struct 328 | { 329 | unsigned short N; 330 | uint32_t ts[2]; 331 | 332 | DataPoint points[1]; 333 | 334 | }DataPoints,*PDataPoints; 335 | 336 | struct UartInfo 337 | { 338 | char type[8]; // uart or vpc 339 | int fd_uart; 340 | char port[128]; 341 | int baudrate; 342 | int *rate_list; 343 | 344 | Parser* hParser; 345 | PubHub* hPublish; 346 | pthread_t thr; 347 | }; 348 | 349 | struct UDPInfo 350 | { 351 | char type[8]; //"udp" 352 | int nnode; 353 | LidarNode lidars[MAX_LIDARS]; 354 | 355 | int fd_udp; 356 | int listen_port; 357 | bool is_group_listener; 358 | pthread_t thr; 359 | 360 | }; 361 | 362 | struct ScriptParam 363 | { 364 | UDPInfo *info; 365 | int id; 366 | }; 367 | 368 | struct KeepAlive 369 | { 370 | uint32_t world_clock; 371 | uint32_t mcu_hz; 372 | uint32_t arrive; 373 | uint32_t delay; 374 | uint32_t reserved[4]; 375 | }; 376 | 377 | struct LidarInfo { 378 | PubHub* pub; 379 | Parser* parser; 380 | char lidar_ip[32]; 381 | int lidar_port; 382 | }; 383 | 384 | struct ConnectArg 385 | { 386 | std::string scan_topics; 387 | std::string cloud_topics; 388 | std::string arg1; 389 | int arg2; 390 | }; 391 | struct Range 392 | { 393 | double min; 394 | double max; 395 | }; 396 | //定制隐藏功能 397 | struct Custom 398 | { 399 | int error_circle; 400 | double error_scale; 401 | bool is_group_listener; 402 | std::string group_ip; 403 | }; 404 | //过滤算法 405 | struct Fitter 406 | { 407 | bool isopen; 408 | int type; 409 | double max_range; 410 | double min_range; 411 | double max_range_difference; 412 | int filter_window; 413 | 414 | }; 415 | struct ArgData 416 | { 417 | std:: string frame_id; 418 | int dev_id; 419 | std::vectorconnectargs; 420 | int raw_bytes; 421 | bool from_zero; 422 | bool output_scan; 423 | bool output_cloud; 424 | bool output_cloud2; 425 | bool inverted; 426 | bool reversed; 427 | bool hard_resample; 428 | bool soft_resample; 429 | bool with_angle_filter; 430 | double min_angle; 431 | double max_angle; 432 | bool output_360; 433 | double min_dist; 434 | double max_dist; 435 | std::vectormasks; 436 | std::string type; 437 | int localport; 438 | int num; 439 | int uuid; 440 | int rpm; 441 | int model; 442 | double resample; 443 | int with_smooth; 444 | int with_deshadow; 445 | int alarm_msg; 446 | int direction; 447 | int unit_is_mm; 448 | int with_confidence; 449 | int ats; 450 | int time_mode; 451 | std::string ntp_ip; 452 | int ntp_port; 453 | int ntp_enable; 454 | bool log_enable; 455 | std::string log_path; 456 | Fitter fitter; 457 | Custom custom; 458 | 459 | }; 460 | 461 | struct DevInfoV101 462 | { 463 | char sign[4]; 464 | uint32_t proto_version; 465 | uint32_t timestamp[2]; 466 | char dev_sn[20]; 467 | char dev_type[16]; 468 | uint32_t version; 469 | uint32_t dev_id; 470 | uint8_t ip[4]; 471 | uint8_t mask[4]; 472 | uint8_t gateway[4]; 473 | uint8_t remote_ip[4]; 474 | 475 | uint16_t remote_udp; 476 | uint16_t port; 477 | uint16_t status; 478 | uint16_t rpm; 479 | 480 | uint16_t freq; 481 | uint16_t unused; 482 | int16_t CpuTemp; 483 | uint16_t InputVolt; 484 | uint8_t alarm[16]; 485 | uint32_t crc; 486 | }; 487 | 488 | struct DevInfo 489 | { 490 | //转速 2个字节 491 | unsigned short rpm; 492 | //启动脉冲个数 2个字节 493 | unsigned short pulse; 494 | //保留 4个字节 495 | char sign[4]; 496 | //版本号 2个字节 497 | unsigned short version; 498 | //ip地址 4个字节 499 | unsigned char ip[4]; 500 | //子网掩码 4个字节 501 | unsigned char mask[4]; 502 | //网关地址 4个字节 503 | unsigned char gateway[4]; 504 | //默认目标IP 4个字节 505 | unsigned char remote_ip[4]; 506 | //默认目标udp端口号 2个字节 507 | unsigned short remote_udp; 508 | //默认UDP对外服务端口号 2个字节 509 | unsigned short port; 510 | //物体分辨率 1个字节 511 | unsigned char fir; 512 | //偏置 6个字节 513 | char zero_offset[6]; 514 | //机器序号 20个字节 515 | char dev_sn[20]; 516 | //机器类型 11个字节 517 | char dev_type[11]; 518 | //IO类型选择 1个字节 519 | char io_type; 520 | //响应圈数 1个字节 521 | unsigned char cir; 522 | //IO功能引脚配置 10个字节 523 | unsigned char io_mux[10]; 524 | }; 525 | struct DevInfo2 526 | { 527 | // 标签 4个字节 528 | char sign[4]; 529 | // 机器序号 20个字节 530 | char dev_sn[20]; 531 | // 机器类型 11个字节 532 | char dev_type[12]; 533 | //版本号 2个字节 534 | unsigned short version; 535 | // ip地址 4个字节 536 | unsigned char ip[4]; 537 | // 子网掩码 4个字节 538 | unsigned char mask[4]; 539 | // 网关地址 4个字节 540 | unsigned char gateway[4]; 541 | // 默认目标IP 4个字节 542 | unsigned char remote_ip[4]; 543 | //默认目标udp端口号 2个字节 544 | unsigned short remote_udp; 545 | // 默认UDP对外服务端口号 2个字节 546 | unsigned short port; 547 | //保留 2个字节 548 | char reserver[2]; 549 | }; 550 | enum ConnType 551 | { 552 | TYPE_COM, 553 | TYPE_UDP_V1, 554 | TYPE_UDP_V2, 555 | TYPE_UDP_V101, 556 | TYPE_VPC 557 | 558 | }; 559 | 560 | struct DevConnInfo 561 | { 562 | ConnType type; 563 | char com_port[128]; 564 | int com_speed; 565 | char conn_ip[32]; 566 | int conn_port; 567 | char mac[32]; 568 | tm ts; 569 | union { 570 | DevInfo v1;// info; 571 | DevInfoV101 v101;// info101; 572 | DevInfo2 v2; 573 | } info; 574 | }; 575 | 576 | //心跳包 16字节报警信息 577 | #define FUN_LV_1 0 578 | #define FUN_LV_2 1 579 | #define FUN_LV_3 2 580 | #define FUN_COVER 3 581 | #define FUN_NO_DATA 4 582 | #define FUN_ZONE_ACTIVE 5 583 | #define FUN_SYS_ERR 6 584 | #define FUN_RUN 7 585 | #define FUN_NET_LINK 8 586 | #define FUN_UPDATING 9 587 | #define FUN_ZERO_POS 10 588 | //#define FUN_HIGH_TEMP 11 589 | //#define FUN_LOW_POWER 12 590 | #define FUN_IS_BOX 13 //new 591 | #define FUN_USB_LINK 14 592 | #define FUN_ZONE_DEFINED 15 593 | 594 | 595 | #endif -------------------------------------------------------------------------------- /bluesea-ros2/sdk/src/bluesea.cpp: -------------------------------------------------------------------------------- 1 | #include "../include/bluesea.h" 2 | BlueSeaLidarDriver::BlueSeaLidarDriver() 3 | { 4 | m_reader = NULL; 5 | m_counterclockwise = false; 6 | memset(&m_cmdlist, 0, sizeof(CommandList)); 7 | } 8 | 9 | BlueSeaLidarDriver::~BlueSeaLidarDriver() 10 | { 11 | } 12 | void BlueSeaLidarDriver::getInitCmds(ArgData &argdata) 13 | { 14 | m_argdata = argdata; 15 | if (argdata.uuid >= 0) 16 | sprintf(m_cmdlist.uuid, "LUUIDH"); 17 | if (argdata.model >= 0) 18 | sprintf(m_cmdlist.model, "LTYPEH"); 19 | if (argdata.rpm >= 0) 20 | sprintf(m_cmdlist.rpm, "LSRPM:%dH", argdata.rpm); 21 | if (argdata.resample >= 0) 22 | { 23 | 24 | if (argdata.resample > 0 && argdata.resample < 1) 25 | sprintf(m_cmdlist.res, "LSRES:%dH", (int)(argdata.resample * 1000)); 26 | else 27 | sprintf(m_cmdlist.res, "LSRES:%dH", (int)argdata.resample); 28 | } 29 | if (argdata.with_smooth >= 0) 30 | { 31 | if (argdata.type == "uart") 32 | sprintf(m_cmdlist.smooth, "LSSS%dH", argdata.with_smooth); 33 | else 34 | sprintf(m_cmdlist.smooth, "LSSMT:%dH", argdata.with_smooth); 35 | } 36 | if (argdata.with_deshadow >= 0) 37 | { 38 | if (argdata.type == "uart") 39 | sprintf(m_cmdlist.fitter, "LFFF%dH", argdata.with_deshadow); 40 | else 41 | sprintf(m_cmdlist.fitter, "LSDSW:%dH", argdata.with_deshadow); 42 | } 43 | if (argdata.alarm_msg >= 0) 44 | sprintf(m_cmdlist.alarm, "LSPST:%dH", argdata.alarm_msg ? 3 : 1); 45 | if (argdata.direction >= 0) 46 | { 47 | if (argdata.type == "uart") 48 | sprintf(m_cmdlist.direction, "LSMCCW:%dH", !(argdata.direction)); 49 | else if (argdata.type == "vpc" || argdata.type == "udp") 50 | sprintf(m_cmdlist.direction, "LSCCW:%dH", argdata.direction); 51 | } 52 | if (argdata.unit_is_mm >= 0) 53 | sprintf(m_cmdlist.unit_mm, "%s", argdata.unit_is_mm ? "LMDMMH" : "LMDCMH"); 54 | if (argdata.with_confidence >= 0) 55 | sprintf(m_cmdlist.confidence, "%s", argdata.with_confidence ? "LOCONH" : "LNCONH"); 56 | 57 | if (argdata.ats >= 0) 58 | { 59 | if (argdata.type == "vpc" || argdata.type == "uart") 60 | sprintf(m_cmdlist.ats, "LSATS:002H"); 61 | else if (argdata.type == "udp") 62 | sprintf(m_cmdlist.ats, "LSATS:001H"); 63 | } 64 | if (argdata.ntp_enable >= 0) 65 | { 66 | if (argdata.type == "udp") 67 | { 68 | bool ret = judgepcIPAddrIsValid(argdata.ntp_ip.data()); 69 | if (!ret) 70 | { 71 | ERROR((int)NTP_IP_FORMAT_ERROR, Error::GetErrorString(NTP_IP_FORMAT_ERROR).c_str()); 72 | return; 73 | } 74 | char ip_1[4]; 75 | char ip_2[4]; 76 | char ip_3[4]; 77 | char ip_4[4]; 78 | ip_1[3] = '\0'; 79 | ip_2[3] = '\0'; 80 | ip_3[3] = '\0'; 81 | ip_4[3] = '\0'; 82 | 83 | int idx[3]; 84 | int index = 0; 85 | int ip_len = argdata.ntp_ip.size(); 86 | const char *ntp_ip = argdata.ntp_ip.data(); 87 | for (int i = 0; i < ip_len; i++) 88 | { 89 | if (ntp_ip[i] == '.') 90 | { 91 | idx[index] = i; 92 | index++; 93 | } 94 | } 95 | 96 | memcpy(ip_1, &ntp_ip[0], idx[0]); 97 | memcpy(ip_2, &ntp_ip[idx[0] + 1], idx[1] - idx[0] - 1); 98 | memcpy(ip_3, &ntp_ip[idx[1] + 1], idx[2] - idx[1] - 1); 99 | memcpy(ip_4, &ntp_ip[idx[2] + 1], ip_len - idx[2]); 100 | sprintf(m_cmdlist.ntp, "LSNTP:%d,%03d.%03d.%03d.%03d,%05dH", argdata.ntp_enable, atoi(ip_1), atoi(ip_2), atoi(ip_3), atoi(ip_4), argdata.ntp_port); 101 | } 102 | } 103 | } 104 | 105 | void BlueSeaLidarDriver::openLidarThread() 106 | { 107 | for (int i = 0; i < m_argdata.num; i++) 108 | { 109 | m_parsers[i] = ParserOpen(m_argdata.raw_bytes, true, m_argdata.dev_id, m_argdata.custom.error_circle, m_argdata.custom.error_scale, m_argdata.from_zero, m_argdata.time_mode, m_cmdlist, (char *)m_argdata.connectargs[i].arg1.c_str(), m_argdata.connectargs[i].arg2); 110 | m_hubs[i] = new PubHub; 111 | m_hubs[i]->nfan = 0; 112 | m_hubs[i]->offsetangle = -1; 113 | pthread_mutex_init(&m_hubs[i]->mtx, NULL); 114 | } 115 | if (m_argdata.type == "uart" || m_argdata.type == "vpc") 116 | { 117 | m_reader = StartUartReader(m_argdata.type.c_str(), m_argdata.connectargs[0].arg1.c_str(), m_argdata.connectargs[0].arg2, m_parsers[0], m_hubs[0]); 118 | } 119 | else if (m_argdata.type == "udp") 120 | { 121 | LidarInfo lidars[MAX_LIDARS]; 122 | for (int i = 0; i < m_argdata.num; i++) 123 | { 124 | lidars[i].parser = m_parsers[i]; 125 | lidars[i].pub = m_hubs[i]; 126 | strcpy(lidars[i].lidar_ip, m_argdata.connectargs[i].arg1.c_str()); 127 | lidars[i].lidar_port = m_argdata.connectargs[i].arg2; 128 | 129 | uint32_t flags = 0; 130 | if (m_argdata.unit_is_mm) 131 | setbit(flags, DF_UNIT_IS_MM); 132 | if (m_argdata.with_confidence) 133 | setbit(flags, DF_WITH_INTENSITY); 134 | 135 | lidars[i].parser->flags = flags; 136 | } 137 | m_reader = StartUDPReader(m_argdata.type.c_str(), m_argdata.localport, m_argdata.custom.is_group_listener, m_argdata.custom.group_ip.c_str(), m_argdata.num, lidars); 138 | } 139 | else if (m_argdata.type == "tcp") 140 | { 141 | m_reader = StartTCPReader(m_argdata.connectargs[0].arg1.c_str(), m_argdata.connectargs[0].arg2, m_parsers[0], m_hubs[0]); 142 | } 143 | } 144 | 145 | bool BlueSeaLidarDriver::sendCmd(std::string topic, std::string cmd, int proto) 146 | { 147 | int idx = -1; 148 | for (unsigned int i = 0; i < m_argdata.connectargs.size(); i++) 149 | { 150 | if (topic == m_argdata.connectargs[i].cloud_topics || topic == m_argdata.connectargs[i].scan_topics) 151 | { 152 | idx = i; 153 | } 154 | } 155 | if (idx < 0) 156 | { 157 | ERROR((int)TOPIC_NO_FIND, Error::GetErrorString(TOPIC_NO_FIND).c_str()); 158 | return false; 159 | } 160 | std::string arg1 = m_argdata.connectargs[idx].arg1; 161 | int arg2 = m_argdata.connectargs[idx].arg2; 162 | 163 | if (m_argdata.type == "uart") 164 | { 165 | return SendUartCmd(m_reader, cmd.size(), (char *)cmd.c_str()); 166 | } 167 | else if (m_argdata.type == "vpc") 168 | { 169 | return SendVpcCmd(m_reader, cmd.size(), (char *)cmd.c_str()); 170 | } 171 | else if (m_argdata.type == "udp") 172 | { 173 | return SendUdpCmd(m_reader, arg1, arg2, cmd, proto); 174 | } 175 | // else if (m_argdata.type == "tcp") 176 | // { 177 | // return SendTcpCmd(m_reader, len, cmd,proto); 178 | // } 179 | return false; 180 | } 181 | 182 | PubHub *BlueSeaLidarDriver::getHub(int i) 183 | { 184 | return m_hubs[i]; 185 | } 186 | Parser *BlueSeaLidarDriver::getParser(int i) 187 | { 188 | return m_parsers[i]; 189 | } 190 | bool BlueSeaLidarDriver::checkIsRun(int index) 191 | { 192 | PubHub *hub = m_hubs[index]; 193 | int checkAngle = (m_argdata.from_zero == true ? 0 : 180); 194 | for (int i = 0; i < hub->nfan; i++) 195 | { 196 | int idx = getFirstidx(*(hub->fans[i]), checkAngle); 197 | if (idx >= 0) 198 | { 199 | hub->offsetangle = hub->fans[i]->angle; 200 | hub->offsetidx = idx; 201 | m_parsers[index]->isrun = true; 202 | DEBUG("lidar start work,offset angle %d offset idx %d raw_bytes:%d\n", hub->offsetangle / 10, hub->offsetidx, m_parsers[index]->raw_mode); 203 | return true; 204 | } 205 | } 206 | return false; 207 | } 208 | 209 | int BlueSeaLidarDriver::GetAllFans(PubHub *pub, ArgData argdata, int8_t &counterclockwise) 210 | { 211 | PubHub *hub = (PubHub *)pub; 212 | RawData *fans[MAX_FANS]; 213 | // int checkAngle = (from_zero == 0 ? 0 : 180); 214 | // 解析出来一帧的数据 215 | pthread_mutex_lock(&hub->mtx); 216 | int cnt = 0, nfan = 0; 217 | for (int i = 1; i < hub->nfan; i++) 218 | { 219 | // DEBUG("%d %d",hub->fans[i]->angle,hub->offsetangle); 220 | if (hub->fans[i]->angle == hub->offsetangle) 221 | { 222 | hub->ts_end[0] = hub->fans[i]->ts[0]; 223 | hub->ts_end[1] = hub->fans[i]->ts[1]; 224 | cnt = i; 225 | break; 226 | } 227 | } 228 | if (cnt > 0) 229 | { 230 | // 起始点位在扇区的中间部分，删除扇区时就需要多保存一个扇区 231 | if (hub->offsetidx > 0) 232 | { 233 | nfan = cnt + 1; 234 | for (int i = 0; i <= cnt; i++) 235 | { 236 | fans[i] = hub->fans[i]; 237 | } 238 | for (int i = cnt; i < hub->nfan; i++) 239 | hub->fans[i - cnt] = hub->fans[i]; 240 | 241 | hub->nfan -= cnt; 242 | } 243 | else 244 | { 245 | nfan = cnt; 246 | for (int i = 0; i < cnt; i++) 247 | { 248 | fans[i] = hub->fans[i]; 249 | } 250 | for (int i = cnt; i < hub->nfan; i++) 251 | hub->fans[i - cnt] = hub->fans[i]; 252 | hub->nfan -= cnt; 253 | } 254 | } 255 | pthread_mutex_unlock(&hub->mtx); 256 | // 一帧数据的丢包检测以及软采样角分辨率的适配 257 | if (cnt > 0) 258 | { 259 | counterclockwise = fans[0]->counterclockwise; 260 | // 如果是-1，则根据类型来区分，串口款都是逆时针，网络和虚拟串口默认为顺时针 261 | if (counterclockwise == -1) 262 | { 263 | if (argdata.type == "vpc" || argdata.type == "udp") 264 | counterclockwise = 0; 265 | else if (argdata.type == "uart") 266 | counterclockwise = 1; 267 | } 268 | bool circle = true; 269 | int total = fans[0]->span; 270 | hub->ts_beg[0] = fans[0]->ts[0]; 271 | hub->ts_beg[1] = fans[0]->ts[1]; 272 | 273 | for (int i = 0; i < nfan - 1; i++) 274 | { 275 | if ((fans[i]->angle + fans[i]->span) % 3600 != fans[i + 1]->angle) 276 | { 277 | circle = false; 278 | } 279 | total += fans[i + 1]->span; 280 | } 281 | //DEBUG("%d %d %d \n", circle,total,hub->offsetidx); 282 | if (!circle || (total != 3600 && hub->offsetidx == 0) || ((total != 3600 + fans[0]->span) && hub->offsetidx > 0)) 283 | { 284 | // clean imcomplent datas 285 | if(!circle) 286 | ERROR((int)DROP_PACKET, Error::GetErrorString(DROP_PACKET).c_str()); 287 | 288 | for (int i = 0; i < cnt; i++) 289 | delete fans[i]; 290 | cnt = 0; 291 | } 292 | } 293 | if (cnt > 0) 294 | { 295 | if (argdata.soft_resample && argdata.resample > 0) 296 | { 297 | for (int i = 0; i < cnt; i++) 298 | { 299 | int NN = fans[i]->span / (10 * argdata.resample); 300 | if (NN < fans[i]->N) 301 | { 302 | // DEBUG("%s %d %d,NN:%d,fans[i]->N:%d %lf\n",__FUNCTION__,__LINE__,cnt,NN,fans[i]->span,resample_res); 303 | resample(fans[i], NN); 304 | } 305 | else if (NN > fans[i]->N) 306 | { 307 | // DEBUG("fan [%d] %d less than %d\n", i, fans[i]->N, NN); 308 | ERROR((int)SAMPLE_TOO_SMALL, Error::GetErrorString(SAMPLE_TOO_SMALL).c_str()); 309 | } 310 | } 311 | } 312 | // 将扇区数据转为点列表 313 | 314 | for (int j = hub->offsetidx; j < fans[0]->N; j++) 315 | { 316 | hub->consume.push_back(fans[0]->points[j]); 317 | } 318 | for (int i = 1; i < cnt; i++) 319 | { 320 | for (int j = 0; j < fans[i]->N; j++) 321 | { 322 | hub->consume.push_back(fans[i]->points[j]); 323 | } 324 | } 325 | for (int j = 0; j < hub->offsetidx; j++) 326 | { 327 | hub->consume.push_back(fans[cnt]->points[j]); 328 | } 329 | for (int i = 0; i < cnt; i++) 330 | delete fans[i]; 331 | 332 | if (cnt > 0) 333 | { 334 | bool res = checkZeroDistance(hub->consume, argdata.custom.error_scale); 335 | if (res) 336 | hub->error_num = 0; 337 | else 338 | { 339 | hub->error_num++; 340 | if (hub->error_num >= argdata.custom.error_circle) 341 | { 342 | // DEBUG("There are many points with a distance of 0 in the current lidar operation"); 343 | ERROR((int)DISTANCE_ZERO_LARGE, Error::GetErrorString(DISTANCE_ZERO_LARGE).c_str()); 344 | hub->error_num = 0; 345 | } 346 | } 347 | } 348 | 349 | int N = hub->consume.size(); 350 | double angle_increment = M_PI * 2 / N; 351 | Fitter *fitter = &argdata.fitter; 352 | if (fitter->isopen) 353 | filter(hub->consume, fitter->max_range * 1000, fitter->min_range * 1000, fitter->max_range_difference * 1000, fitter->filter_window, angle_increment); 354 | 355 | counterclockwise = true; 356 | m_counterclockwise = counterclockwise; 357 | // DEBUG("%d",m_counterclockwise); 358 | } 359 | return cnt; 360 | } 361 | bool BlueSeaLidarDriver::GetFan(PubHub *pub, bool with_resample, double resample_res, RawData **fans) 362 | { 363 | bool got = false; 364 | PubHub *hub = (PubHub *)pub; 365 | if (hub->nfan > 0) 366 | { 367 | pthread_mutex_lock(&hub->mtx); 368 | fans[0] = hub->fans[0]; 369 | for (int i = 1; i < hub->nfan; i++) 370 | hub->fans[i - 1] = hub->fans[i]; 371 | hub->nfan--; 372 | got = true; 373 | pthread_mutex_unlock(&hub->mtx); 374 | } 375 | else 376 | return false; 377 | 378 | if (with_resample) // && resample_res > 0.05) 379 | { 380 | int NN = fans[0]->span / (10 * resample_res); 381 | 382 | if (NN < fans[0]->N) 383 | { 384 | resample(fans[0], NN); 385 | } 386 | else if (NN > fans[0]->N) 387 | { 388 | // DEBUG("fan %d less than %d\n", fans[0]->N, NN); 389 | ERROR((int)SAMPLE_TOO_SMALL, Error::GetErrorString(SAMPLE_TOO_SMALL).c_str()); 390 | } 391 | } 392 | return got; 393 | } 394 | double BlueSeaLidarDriver::ROSAng(double ang) 395 | { 396 | if (m_counterclockwise + m_argdata.reversed != 1) 397 | { 398 | return ang > 180 ? 360 - ang : -ang; 399 | } 400 | else 401 | { 402 | return ang > 180 ? ang - 360 : ang; 403 | } 404 | } 405 | 406 | int BlueSeaLidarDriver::GetCount(std::vector data, double min_deg, double max_deg, double &min_pos, double &max_pos) 407 | { 408 | int N = 0; 409 | 410 | for (unsigned int i = 0; i < data.size(); i++) 411 | { 412 | double deg = ROSAng(data[i].degree); 413 | 414 | if (deg < min_deg || deg > max_deg) 415 | continue; 416 | if (N == 0) 417 | { 418 | min_pos = deg; 419 | max_pos = deg; 420 | } 421 | else 422 | { 423 | if (min_pos > deg) 424 | min_pos = deg; 425 | if (max_pos < deg) 426 | max_pos = deg; 427 | } 428 | N++; 429 | } 430 | 431 | // printf("angle filter [%f, %f] %d to %d, [%f, %f]\n", min_deg, max_deg, 1, N, min_pos, max_pos); 432 | return N; 433 | } 434 | 435 | bool judgepcIPAddrIsValid(const char *pcIPAddr) 436 | { 437 | int iDots = 0; /* 字符.的个数 */ 438 | int iSetions = 0; /* pcIPAddr 每一部分总和（0-255）*/ 439 | 440 | if (NULL == pcIPAddr || *pcIPAddr == '.') 441 | { /*排除输入参数为NULL, 或者一个字符为'.'的字符串*/ 442 | return false; 443 | } 444 | 445 | /* 循环取每个字符进行处理 */ 446 | while (*pcIPAddr) 447 | { 448 | if (*pcIPAddr == '.') 449 | { 450 | iDots++; 451 | /* 检查 pcIPAddr 是否合法 */ 452 | if (iSetions >= 0 && iSetions <= 255) 453 | { 454 | iSetions = 0; 455 | pcIPAddr++; 456 | continue; 457 | } 458 | else 459 | { 460 | return false; 461 | } 462 | } 463 | else if (*pcIPAddr >= '0' && *pcIPAddr <= '9') /*判断是不是数字*/ 464 | { 465 | iSetions = iSetions * 10 + (*pcIPAddr - '0'); /*求每一段总和*/ 466 | } 467 | else 468 | { 469 | return false; 470 | } 471 | pcIPAddr++; 472 | } 473 | 474 | /* 判断最后一段是否有值如：1.1.1. */ 475 | if ((*pcIPAddr == '\0') && (*(pcIPAddr - 1) == '.')) 476 | { 477 | return false; 478 | } 479 | 480 | /* 判断最后一段是否合法 */ 481 | if (iSetions >= 0 && iSetions <= 255) 482 | { 483 | if (iDots == 3) 484 | { 485 | return true; 486 | } 487 | } 488 | 489 | return false; 490 | } -------------------------------------------------------------------------------- /bluesea-ros2/sdk/src/udp_reader.cpp: -------------------------------------------------------------------------------- 1 | #include "../include/alarm.h" 2 | #include "../include/reader.h" 3 | 4 | void send_cmd_udp(int fd_udp, const char *dev_ip, int dev_port, int cmd, int sn, int len, const void *snd_buf) 5 | { 6 | char buffer[2048]; 7 | CmdHeader *hdr = (CmdHeader *)buffer; 8 | hdr->sign = 0x484c; 9 | hdr->cmd = cmd; 10 | hdr->sn = sn; 11 | 12 | len = ((len + 3) >> 2) * 4; 13 | 14 | hdr->len = len; 15 | 16 | memcpy(buffer + sizeof(CmdHeader), snd_buf, len); 17 | 18 | unsigned int *pcrc = (unsigned int *)(buffer + sizeof(CmdHeader) + len); 19 | pcrc[0] = stm32crc((unsigned int *)(buffer + 0), len / 4 + 2); 20 | 21 | sockaddr_in to; 22 | to.sin_family = AF_INET; 23 | to.sin_addr.s_addr = inet_addr(dev_ip); 24 | to.sin_port = htons(dev_port); 25 | 26 | int len2 = len + sizeof(CmdHeader) + 4; 27 | 28 | sendto(fd_udp, buffer, len2, 0, (struct sockaddr *)&to, sizeof(struct sockaddr)); 29 | } 30 | 31 | bool udp_talk_S_PACK(int fd_udp, const char *ip, int port, int n, const char *cmd, void *result) 32 | { 33 | DEBUG("%s", cmd); 34 | unsigned short sn = rand(); 35 | send_cmd_udp(fd_udp, ip, port, 0x0053, sn, n, cmd); 36 | 37 | int nr = 0; 38 | for (int i = 0; i < 1000; i++) 39 | { 40 | fd_set fds; 41 | FD_ZERO(&fds); 42 | 43 | FD_SET(fd_udp, &fds); 44 | 45 | struct timeval to = {3, 0}; 46 | int ret = select(fd_udp + 1, &fds, NULL, NULL, &to); 47 | if (ret <= 0) 48 | { 49 | ERROR((int)SOCKET_ERR, Error::GetErrorString(SOCKET_ERR).c_str()); 50 | return false; 51 | } 52 | // read UDP data 53 | if (FD_ISSET(fd_udp, &fds)) 54 | { 55 | nr++; 56 | sockaddr_in addr; 57 | socklen_t sz = sizeof(addr); 58 | char buf[1024] = {0}; 59 | int nr = recvfrom(fd_udp, buf, sizeof(buf), 0, (struct sockaddr *)&addr, &sz); 60 | if (nr > 0) 61 | { 62 | CmdHeader *hdr = (CmdHeader *)buf; 63 | if (hdr->sign != 0x484c || hdr->sn != sn) 64 | continue; 65 | memcpy(result, buf + 8, 2); 66 | return true; 67 | } 68 | } 69 | } 70 | 71 | // DEBUG("read %d packets, not response\n", nr); 72 | ERROR((int)CMD_NO_ANSWER, Error::GetErrorString(CMD_NO_ANSWER).c_str()); 73 | return false; 74 | } 75 | 76 | bool udp_talk_C_PACK(int fd_udp, const char *lidar_ip, int lidar_port, 77 | int n, const char *cmd, 78 | int nhdr, const char *hdr_str, 79 | int nfetch, char *fetch) 80 | { 81 | DEBUG("send command %s", cmd); 82 | unsigned short sn = rand(); 83 | send_cmd_udp(fd_udp, lidar_ip, lidar_port, 0x0043, sn, n, cmd); 84 | 85 | time_t t0 = time(NULL); 86 | int ntry = 0; 87 | while (time(NULL) < t0 + 3 && ntry < 1000) 88 | { 89 | fd_set fds; 90 | FD_ZERO(&fds); 91 | FD_SET(fd_udp, &fds); 92 | 93 | struct timeval to = {1, 0}; 94 | int ret = select(fd_udp + 1, &fds, NULL, NULL, &to); 95 | 96 | if (ret < 0) 97 | { 98 | // DEBUG("select error\n"); 99 | ERROR((int)SOCKET_ERR, Error::GetErrorString(SOCKET_ERR).c_str()); 100 | return false; 101 | } 102 | if (ret == 0) 103 | { 104 | continue; 105 | } 106 | 107 | // read UDP data 108 | if (FD_ISSET(fd_udp, &fds)) 109 | { 110 | ntry++; 111 | sockaddr_in addr; 112 | socklen_t sz = sizeof(addr); 113 | 114 | char buf[1024] = {0}; 115 | int nr = recvfrom(fd_udp, buf, sizeof(buf), 0, (struct sockaddr *)&addr, &sz); 116 | if (nr > 0) 117 | { 118 | CmdHeader *hdr = (CmdHeader *)buf; 119 | if (hdr->sign != 0x484c || hdr->sn != sn) 120 | continue; 121 | 122 | char *payload = buf + sizeof(CmdHeader); 123 | for (int i = 0; i < nr - nhdr - 1; i++) 124 | { 125 | if (memcmp(payload + i, hdr_str, nhdr) == 0) 126 | { 127 | if (nfetch > 0) 128 | { 129 | memset(fetch, 0, nfetch); 130 | for (int j = 0; j < nfetch && i + nhdr + j < nr; j++) 131 | fetch[j] = payload[i + nhdr + j]; 132 | } 133 | return true; 134 | } 135 | } 136 | } 137 | } 138 | } 139 | // DEBUG("read %d packets, not response\n", ntry); 140 | ERROR((int)CMD_NO_ANSWER, Error::GetErrorString(CMD_NO_ANSWER).c_str()); 141 | return false; 142 | } 143 | 144 | void *UdpThreadProc(void *p) 145 | { 146 | UDPInfo *info = (UDPInfo *)p; 147 | int fd_udp = info->fd_udp; 148 | char buf[1024]; 149 | 150 | timeval tv; 151 | gettimeofday(&tv, NULL); 152 | 153 | time_t tto = tv.tv_sec + 1; 154 | uint32_t delay = 0; 155 | // bool hasdata = false; 156 | // uint32_t nodata_count = 0; 157 | if (!info->is_group_listener) 158 | { 159 | for (int i = 0; i < info->nnode; i++) 160 | { 161 | EEpromV101 param; 162 | setup_lidar_udp(info->fd_udp, info->lidars[i].hParser, param); 163 | std::string model = stringfilter((char *)param.dev_type, 16); 164 | DEBUG("%s", model.c_str()); 165 | } 166 | } 167 | while (1) 168 | { 169 | gettimeofday(&tv, NULL); 170 | if (tv.tv_sec > tto && !info->is_group_listener) 171 | { 172 | for (int i = 0; i < info->nnode; i++) 173 | { 174 | Parser *parser = (Parser *)info->lidars[i].hParser; 175 | int cmdLength = strlen(parser->cmd.ntp); 176 | if (cmdLength <= 0) 177 | { 178 | KeepAlive alive; 179 | gettimeofday(&tv, NULL); 180 | alive.world_clock = (tv.tv_sec % 3600) * 1000 + tv.tv_usec / 1000; 181 | alive.delay = delay; 182 | send_cmd_udp(info->fd_udp, info->lidars[i].ip, info->lidars[i].port, 0x4b41, rand(), sizeof(alive), &alive); 183 | } 184 | 185 | tto = tv.tv_sec + 1; 186 | if (info->lidars[i].hasdata == true) 187 | info->lidars[i].hasdata = false; 188 | else 189 | { 190 | info->lidars[i].nodata_count++; 191 | DEBUG("lidar data is not recv,count:%d\n", info->lidars[i].nodata_count); 192 | if (udp_talk_C_PACK(info->fd_udp, parser->ip, parser->port, 6, "LUUIDH", 11, "PRODUCT SN:", 20, buf)) 193 | { 194 | std::string sn = stringfilter(buf, 20); 195 | DEBUG("uuid:%s", sn.c_str()); 196 | } 197 | } 198 | } 199 | } 200 | 201 | 202 | fd_set fds; 203 | FD_ZERO(&fds); 204 | FD_SET(fd_udp, &fds); 205 | struct timeval to = {1, 0}; 206 | int ret = select(fd_udp + 1, &fds, NULL, NULL, &to); 207 | if (ret == 0) 208 | { 209 | usleep(1000); 210 | continue; 211 | } 212 | 213 | if (ret < 0) 214 | { 215 | DEBUG("%d select error,thread end\n",fd_udp); 216 | break; 217 | } 218 | // read UDP data 219 | if (FD_ISSET(fd_udp, &fds)) 220 | { 221 | sockaddr_in addr; 222 | socklen_t sz = sizeof(addr); 223 | 224 | int nr = recvfrom(fd_udp, buf, sizeof(buf), 0, (struct sockaddr *)&addr, &sz); 225 | if (nr > 0) 226 | { 227 | int id = -1; 228 | for (int i = 0; i < info->nnode; i++) 229 | { 230 | // char recv_ip[16]={0}; 231 | // strcpy(recv_ip, (char *)inet_ntoa(addr.sin_addr)); 232 | if (strcmp((char *)inet_ntoa(addr.sin_addr), info->lidars[i].ip) == 0) 233 | { 234 | id = i; 235 | break; 236 | } 237 | } 238 | if (id == -1) 239 | { 240 | // continue; 241 | // DEBUG("packet from unknown address %s\n", inet_ntoa(addr.sin_addr)); 242 | } 243 | else if (buf[0] == 0x4c && buf[1] == 0x48 && buf[2] == ~0x41 && buf[3] == ~0x4b) 244 | { 245 | // if (nr == sizeof(KeepAlive) + 12) 246 | // { 247 | // uint32_t clock = (tv.tv_sec % 3600) * 1000 + tv.tv_usec / 1000; 248 | // KeepAlive *ka = (KeepAlive *)(buf + 8); 249 | // if (clock >= ka->world_clock) 250 | // delay = clock - ka->world_clock; 251 | // else 252 | // delay = clock + 36000000 - ka->world_clock; 253 | // } 254 | } 255 | else 256 | { 257 | RawData *fans[MAX_FANS]; 258 | int nfan = ParserRun(info->lidars[id], nr, (uint8_t *)buf, &(fans[0])); 259 | if (nfan > 0) 260 | { 261 | PublishData(info->lidars[id].hPublish, nfan, fans); 262 | if (info->lidars[id].nodata_count > 1) 263 | DEBUG("lidar data is recovered,count:%d\n", info->lidars[id].nodata_count); 264 | 265 | info->lidars[id].hasdata = true; 266 | info->lidars[id].nodata_count = 0; 267 | } 268 | } 269 | } 270 | } 271 | } 272 | return NULL; 273 | } 274 | 275 | int AddLidar(HReader hr, const char *lidar_ip, unsigned short lidar_port, Parser *hParser, PubHub *hPub) 276 | { 277 | UDPInfo *info = (UDPInfo *)hr; 278 | 279 | if (info->nnode >= MAX_LIDARS) 280 | { 281 | // DEBUG("There has %d lidars\n", info->nnode); 282 | ERROR((int)LIDAR_NUM_LARGE, Error::GetErrorString(LIDAR_NUM_LARGE).c_str()); 283 | return -1; 284 | } 285 | 286 | info->lidars[info->nnode].hParser = hParser; 287 | info->lidars[info->nnode].hPublish = hPub; 288 | strcpy(info->lidars[info->nnode].ip, lidar_ip); 289 | info->lidars[info->nnode].port = lidar_port; 290 | DEBUG("add lidar %s:%d", lidar_ip, lidar_port); 291 | return info->nnode++; 292 | } 293 | 294 | HReader StartUDPReader(const char *type, unsigned short listen_port, bool is_group_listener, const char *group_ip, 295 | int lidar_count, const LidarInfo *lidars) 296 | { 297 | UDPInfo *info = new UDPInfo; 298 | memset(info, 0, sizeof(UDPInfo)); 299 | 300 | for (int i = 0; i < lidar_count; i++) 301 | { 302 | AddLidar(info, lidars[i].lidar_ip, lidars[i].lidar_port, lidars[i].parser, lidars[i].pub); 303 | } 304 | 305 | strcpy(info->type, type); 306 | info->listen_port = listen_port; 307 | info->is_group_listener = is_group_listener; 308 | // open UDP port 309 | info->fd_udp = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP); 310 | 311 | int one = 1; 312 | setsockopt(info->fd_udp, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)); 313 | 314 | sockaddr_in addr; 315 | addr.sin_family = AF_INET; 316 | addr.sin_port = htons(listen_port); 317 | addr.sin_addr.s_addr = htonl(INADDR_ANY); 318 | 319 | int rt = ::bind(info->fd_udp, (struct sockaddr *)&addr, sizeof(addr)); 320 | if (rt != 0) 321 | { 322 | // DEBUG("bind port %d failed\n", listen_port); 323 | ERROR((int)PORT_BIND_ERR, Error::GetErrorString(PORT_BIND_ERR).c_str()); 324 | } 325 | 326 | DEBUG("start udp listen port %d udp %d\n", listen_port, info->fd_udp); 327 | 328 | if (is_group_listener) 329 | { 330 | ip_mreq group; 331 | memset(&group, 0, sizeof(group)); 332 | group.imr_multiaddr.s_addr = inet_addr(group_ip); 333 | group.imr_interface.s_addr = INADDR_ANY; 334 | 335 | int rt = setsockopt(info->fd_udp, IPPROTO_IP, 336 | IP_ADD_MEMBERSHIP, (char *)&group, 337 | sizeof(group)); 338 | 339 | DEBUG("Adding to multicast group %s %s\n", group_ip, rt < 0 ? "fail!" : "ok"); 340 | } 341 | pthread_create(&info->thr, NULL, UdpThreadProc, info); 342 | return info; 343 | } 344 | 345 | bool SendUdpCmd(HReader hr, std::string lidarIp, int port, std::string cmd, int proto) 346 | { 347 | UDPInfo *info = (UDPInfo *)hr; 348 | if (!info || info->fd_udp <= 0 || info->is_group_listener) 349 | return false; 350 | 351 | for (int i = 0; i < info->nnode; i++) 352 | { 353 | if (lidarIp == info->lidars[i].ip && port == info->lidars[i].port) 354 | { 355 | send_cmd_udp(info->fd_udp, info->lidars[i].ip, info->lidars[i].port, proto, rand(), cmd.size(), cmd.c_str()); 356 | return true; 357 | } 358 | } 359 | return false; 360 | } 361 | 362 | bool udp_talk_GS_PACK(int fd_udp, const char *ip, int port, int n, const char *cmd, void *result) 363 | { 364 | unsigned short sn = rand(); 365 | send_cmd_udp(fd_udp, ip, port, 0x4753, sn, n, cmd); 366 | 367 | int nr = 0; 368 | for (int i = 0; i < 1000; i++) 369 | { 370 | fd_set fds; 371 | FD_ZERO(&fds); 372 | 373 | FD_SET(fd_udp, &fds); 374 | 375 | struct timeval to = {1, 0}; 376 | int ret = select(fd_udp + 1, &fds, NULL, NULL, &to); 377 | 378 | if (ret <= 0) 379 | { 380 | ERROR((int)SOCKET_ERR, Error::GetErrorString(SOCKET_ERR).c_str()); 381 | return false; 382 | } 383 | 384 | // read UDP data 385 | if (FD_ISSET(fd_udp, &fds)) 386 | { 387 | nr++; 388 | sockaddr_in addr; 389 | socklen_t sz = sizeof(addr); 390 | 391 | char buf[1024] = {0}; 392 | int nr = recvfrom(fd_udp, buf, sizeof(buf), 0, (struct sockaddr *)&addr, &sz); 393 | if (nr > 0) 394 | { 395 | CmdHeader *hdr = (CmdHeader *)buf; 396 | if (hdr->sign != 0x484c || hdr->sn != sn) 397 | continue; 398 | 399 | memcpy(result, buf + sizeof(CmdHeader), sizeof(EEpromV101)); 400 | // EEpromV101 t; 401 | // memcpy(&t, result, sizeof(EEpromV101)); 402 | return true; 403 | } 404 | } 405 | } 406 | 407 | ERROR((int)CMD_NO_ANSWER, Error::GetErrorString(CMD_NO_ANSWER).c_str()); 408 | // DEBUG("read %d packets, not response\n", nr); 409 | return false; 410 | } 411 | 412 | void StopUDPReader(HReader hr) 413 | { 414 | UDPInfo *info = (UDPInfo *)hr; 415 | close(info->fd_udp); 416 | // DEBUG("stop udp reader\n"); 417 | // info->should_exit = true; 418 | // sleep(1); 419 | pthread_join(info->thr, NULL); 420 | 421 | delete info; 422 | } 423 | bool setup_lidar_udp(int handle, Parser *hP, EEpromV101 ¶m) 424 | { 425 | // 增加全局变量 426 | Parser *parser = (Parser *)hP; 427 | unsigned int index = 5; 428 | int cmdLength; 429 | char buf[32]; 430 | char result[3] = {0}; 431 | result[2] = '\0'; 432 | 433 | for (unsigned int i = 0; i < index; i++) 434 | { 435 | cmdLength = strlen(parser->cmd.ats); 436 | if (cmdLength <= 0) 437 | break; 438 | if (udp_talk_S_PACK(handle, parser->ip, parser->port, cmdLength, parser->cmd.ats, result)) 439 | { 440 | DEBUG("set ats %s", result); 441 | break; 442 | } 443 | } 444 | 445 | for (unsigned int i = 0; i < index; i++) 446 | { 447 | cmdLength = strlen(parser->cmd.uuid); 448 | if (cmdLength <= 0) 449 | break; 450 | if (udp_talk_C_PACK(handle, parser->ip, parser->port, cmdLength, parser->cmd.uuid, 11, "PRODUCT SN:", 20, buf)) 451 | { 452 | std::string sn = stringfilter(buf, 20); 453 | DEBUG("uuid:%s", sn.c_str()); 454 | break; 455 | } 456 | else if (udp_talk_C_PACK(handle, parser->ip, parser->port, cmdLength, parser->cmd.uuid, 10, "VENDOR ID:", 20, buf)) 457 | { 458 | std::string sn = stringfilter(buf, 20); 459 | DEBUG("uuid:%s", sn.c_str()); 460 | break; 461 | } 462 | } 463 | 464 | // enable/disable shadow filter 465 | for (unsigned int i = 0; i < index; i++) 466 | { 467 | cmdLength = strlen(parser->cmd.fitter); 468 | if (cmdLength <= 0) 469 | break; 470 | 471 | if (udp_talk_S_PACK(handle, parser->ip, parser->port, cmdLength, parser->cmd.fitter, result)) 472 | { 473 | DEBUG("set LiDAR shadow filter %s %s", parser->cmd.fitter, result); 474 | break; 475 | } 476 | } 477 | // enable/disable smooth 478 | for (unsigned int i = 0; i < index; i++) 479 | { 480 | cmdLength = strlen(parser->cmd.smooth); 481 | if (cmdLength <= 0) 482 | break; 483 | 484 | if (udp_talk_S_PACK(handle, parser->ip, parser->port, cmdLength, parser->cmd.smooth, result)) 485 | { 486 | DEBUG("set LiDAR smooth %s %s", parser->cmd.smooth, result); 487 | break; 488 | } 489 | } 490 | // setup rpm 491 | for (unsigned int i = 0; i < index; i++) 492 | { 493 | cmdLength = strlen(parser->cmd.rpm); 494 | if (cmdLength <= 0) 495 | break; 496 | 497 | if (udp_talk_S_PACK(handle, parser->ip, parser->port, cmdLength, parser->cmd.rpm, result)) 498 | { 499 | DEBUG("%s %s", parser->cmd.rpm, result); 500 | break; 501 | } 502 | } 503 | 504 | for (unsigned int i = 0; i < index; i++) 505 | { 506 | cmdLength = strlen(parser->cmd.res); 507 | if (cmdLength <= 0) 508 | break; 509 | 510 | if (udp_talk_C_PACK(handle, parser->ip, parser->port, cmdLength, parser->cmd.res, 2, "OK", 0, NULL)) 511 | { 512 | DEBUG("%s OK", parser->cmd.res); 513 | break; 514 | } 515 | } 516 | 517 | // enable/disable alaram message uploading 518 | for (unsigned int i = 0; i < index; i++) 519 | { 520 | cmdLength = strlen(parser->cmd.alarm); 521 | if (cmdLength <= 0) 522 | break; 523 | if (udp_talk_S_PACK(handle, parser->ip, parser->port, cmdLength, parser->cmd.alarm, result)) 524 | { 525 | DEBUG("set alarm_msg %s", result); 526 | break; 527 | } 528 | } 529 | 530 | for (unsigned int i = 0; i < index; i++) 531 | { 532 | cmdLength = strlen(parser->cmd.direction); 533 | if (cmdLength <= 0) 534 | break; 535 | if (udp_talk_S_PACK(handle, parser->ip, parser->port, cmdLength, parser->cmd.direction, result)) 536 | { 537 | DEBUG("set direction %s", result); 538 | break; 539 | } 540 | } 541 | for (unsigned int i = 0; i < index; i++) 542 | { 543 | cmdLength = strlen(parser->cmd.confidence); 544 | if (cmdLength <= 0) 545 | break; 546 | if (udp_talk_C_PACK(handle, parser->ip, parser->port, cmdLength, parser->cmd.confidence, 2, "OK", 0, NULL)) 547 | { 548 | DEBUG("set confidence %s", result); 549 | break; 550 | } 551 | } 552 | for (unsigned int i = 0; i < index; i++) 553 | { 554 | cmdLength = strlen(parser->cmd.ntp); 555 | if (cmdLength <= 0) 556 | break; 557 | if (udp_talk_S_PACK(handle, parser->ip, parser->port, cmdLength, parser->cmd.ntp, result)) 558 | { 559 | DEBUG("set ntp %s", result); 560 | if (result[0] == 'O' && result[1] == 'K' && parser->cmd.ntp[6] == '1') 561 | timestampMode(1); 562 | else 563 | timestampMode(0); 564 | 565 | break; 566 | } 567 | } 568 | for (unsigned int i = 0; i < index; i++) 569 | { 570 | if (udp_talk_GS_PACK(handle, parser->ip, parser->port, 6, "xxxxxx", ¶m)) 571 | { 572 | break; 573 | } 574 | } 575 | return true; 576 | } 577 | -------------------------------------------------------------------------------- /bluesea-ros2/sdk/src/uart_reader.cpp: -------------------------------------------------------------------------------- 1 | #include "../include/reader.h" 2 | 3 | extern "C" int change_baud(int fd, unsigned int baud); 4 | 5 | bool uart_talk(int fd, int n, const char *cmd, int nhdr, const char *hdr_str, int nfetch, char *fetch, int waittime, int CacheLength) 6 | { 7 | DEBUG("send command : %s", cmd); 8 | write(fd, cmd, n); 9 | 10 | char *buf = new char[CacheLength]; 11 | int nr = read(fd, buf, sizeof(buf)); 12 | int idx = waittime; 13 | 14 | while (nr < CacheLength) 15 | { 16 | int n = read(fd, buf + nr, CacheLength - nr); 17 | // printf(" fd %d %d \n",n,nr); 18 | if (n > 0) 19 | { 20 | nr += n; 21 | idx = waittime; 22 | } 23 | else if (n == 0) 24 | { 25 | idx--; 26 | usleep(1000); 27 | if (idx == 0) 28 | { 29 | // printf("read 0 byte max index break\n"); 30 | break; 31 | } 32 | } 33 | } 34 | // if(idx>0) 35 | // printf("read max byte break\n"); 36 | for (int i = 0; i < CacheLength - nhdr - nfetch; i++) 37 | { 38 | if (memcmp(buf + i, hdr_str, nhdr) == 0 && nhdr > 0) 39 | { 40 | if (nfetch > 0) 41 | { 42 | if (strcmp(cmd, "LVERSH") == 0 || strcmp(cmd, "LUUIDH") == 0 || strcmp(cmd, "LTYPEH") == 0 || strcmp(cmd, "LQAZNH") == 0 || strcmp(cmd, "LQPSTH") == 0 || strcmp(cmd, "LQNPNH") == 0 || strcmp(cmd, "LQOUTH") == 0 || strcmp(cmd, "LQCIRH") == 0 || strcmp(cmd, "LQFIRH") == 0 || strcmp(cmd, "LQSRPMH") == 0 || strcmp(cmd, "LQSMTH") == 0 || strcmp(cmd, "LQDSWH") == 0 || strcmp(cmd, "LQZTPH") == 0 || strcmp(cmd, "LQSAFH") == 0 || strcmp(cmd, "LQZIRH") == 0) 43 | { 44 | memcpy(fetch, buf + i + nhdr, nfetch); 45 | fetch[nfetch] = '\0'; 46 | } 47 | else if (strstr(cmd, "LSRPM") != NULL) 48 | { 49 | if (buf[i + nhdr + 1] == 'O' && buf[i + nhdr + 2] == 'K') 50 | { 51 | strncpy(fetch, "OK", 3); 52 | fetch[2] = '\0'; 53 | } 54 | else if (buf[i + nhdr + 1] == 'e' && buf[i + nhdr + 2] == 'r') 55 | { 56 | strncpy(fetch, "NG", 3); 57 | fetch[2] = '\0'; 58 | } 59 | } 60 | else 61 | { 62 | strncpy(fetch, "OK", 3); 63 | fetch[2] = '\0'; 64 | } 65 | } 66 | return true; 67 | } 68 | else if (memcmp(buf + i, cmd, n) == 0) 69 | { 70 | if (nfetch > 0) 71 | { 72 | memcpy(fetch, buf + i + n + 1, 2); 73 | if (buf[i + n + 1] == 'E' && buf[i + n + 2] == 'R') 74 | { 75 | fetch[0] = 'N'; 76 | fetch[1] = 'G'; 77 | } 78 | fetch[2] = 0; 79 | } 80 | return true; 81 | } 82 | else if (memcmp(buf + i, "unsupport", 9) == 0) 83 | { 84 | if (nfetch > 0) 85 | { 86 | strcpy(fetch, "unsupport"); 87 | fetch[10] = 0; 88 | } 89 | return true; 90 | } 91 | } 92 | delete[] buf; 93 | 94 | ERROR((int)CMD_NO_ANSWER, Error::GetErrorString(CMD_NO_ANSWER).c_str()); 95 | // DEBUG("read %d bytes, not found %s", nr, hdr_str); 96 | return false; 97 | } 98 | 99 | bool vpc_talk(int hcom, int mode, short sn, int len, const char *cmd, int nfetch, void *result) 100 | { 101 | DEBUG("USB send command : %s\n", cmd); 102 | char buffer[2048]; 103 | CmdHeader *hdr = (CmdHeader *)buffer; 104 | hdr->sign = 0x484c; 105 | hdr->cmd = mode; 106 | hdr->sn = sn; 107 | len = ((len + 3) >> 2) * 4; 108 | 109 | hdr->len = len; 110 | 111 | memcpy(buffer + sizeof(CmdHeader), cmd, len); 112 | 113 | unsigned int *pcrc = (unsigned int *)(buffer + sizeof(CmdHeader) + len); 114 | pcrc[0] = stm32crc((unsigned int *)(buffer + 0), len / 4 + 2); 115 | // pcrc[0] = BaseAPI::stm32crc((unsigned int*)(buffer + 0), len / 4 + 2); 116 | 117 | int len2 = len + sizeof(CmdHeader) + 4; 118 | int nr = write(hcom, buffer, len2); 119 | unsigned char buf[2048]; 120 | int index = 10; 121 | // 4C 48 BC FF xx xx xx xx result 122 | // 读取之后的10*2048个长度，如果不存在即判定失败 123 | while (index--) 124 | { 125 | nr = read(hcom, buf, sizeof(buf)); 126 | while (nr < (int)sizeof(buf)) 127 | { 128 | int n = read(hcom, buf + nr, sizeof(buf) - nr); 129 | if (n > 0) 130 | nr += n; 131 | } 132 | 133 | for (int i = 0; i < (int)sizeof(buf) - nfetch; i++) 134 | { 135 | if (mode == C_PACK) 136 | { 137 | char *fetch = (char *)result; 138 | if (buf[i] == 0x4C && buf[i + 1] == 0x48 && buf[i + 2] == 0xBC && buf[i + 3] == 0xFF) 139 | { 140 | /*int packSN = ((unsigned int)buf[i + 5] << 8) | (unsigned int)buf[i + 4]; 141 | if (packSN != sn) 142 | continue;*/ 143 | 144 | for (int j = 0; j < nfetch; j++) 145 | { 146 | if ((buf[i + j + 8] >= 33 && buf[i + j + 8] <= 127)) 147 | { 148 | fetch[j] = buf[i + j + 8]; 149 | } 150 | else 151 | { 152 | fetch[j] = ' '; 153 | } 154 | } 155 | fetch[nfetch] = 0; 156 | return true; 157 | } 158 | } 159 | else if (mode == S_PACK) 160 | { 161 | if ((buf[i + 2] == 0xAC && buf[i + 3] == 0xB8) || (buf[i + 2] == 0xAC && buf[i + 3] == 0xff)) 162 | { 163 | // printf("%02x %02x\n", buf[i + 2], buf[i + 3]); 164 | // 随机码判定 165 | short packSN = ((unsigned char)buf[i + 5] << 8) | (unsigned char)buf[i + 4]; 166 | if (packSN != sn) 167 | continue; 168 | 169 | memcpy(result, buf + i + 8, nfetch); 170 | return true; 171 | } 172 | } 173 | } 174 | } 175 | // printf("read %d bytes, not found %s\n", nr, cmd); 176 | ERROR((int)CMD_NO_ANSWER, Error::GetErrorString(CMD_NO_ANSWER).c_str()); 177 | return false; 178 | } 179 | 180 | int open_serial_port(const char *port, unsigned int baudrate) 181 | { 182 | int fd = open(port, O_RDWR | O_NOCTTY | O_NDELAY); 183 | if (fd < 0) 184 | { 185 | // DEBUG("open %s error", port); 186 | ERROR((int)COM_OPEN_ERR, Error::GetErrorString(COM_OPEN_ERR).c_str()); 187 | return -1; 188 | } 189 | 190 | int ret; 191 | struct termios attrs; 192 | 193 | tcflush(fd, TCIOFLUSH); 194 | 195 | /* get current attrs */ 196 | ret = tcgetattr(fd, &attrs); 197 | if (ret < 0) 198 | { 199 | // DEBUG("get attrs failed"); 200 | ERROR((int)GET_COM_ARG_ERR, Error::GetErrorString(GET_COM_ARG_ERR).c_str()); 201 | return -1; 202 | } 203 | 204 | /* set speed */ 205 | // int speed = B230400; 206 | // if (baudrate == 115200) speed = B115200; 207 | 208 | // ret = cfsetispeed(&attrs, speed); //[baudrate]); 209 | // ret |= cfsetospeed(&attrs, speed); //[baudrate]); 210 | 211 | /* enable recieve and set as local line */ 212 | attrs.c_cflag |= (CLOCAL | CREAD); 213 | 214 | /* set data bits */ 215 | attrs.c_cflag &= ~CSIZE; 216 | attrs.c_cflag |= CS8; 217 | 218 | /* set parity */ 219 | if (1) 220 | { // parity == UART_POFF) { 221 | attrs.c_cflag &= ~PARENB; // disable parity 222 | attrs.c_iflag &= ~INPCK; 223 | } 224 | else 225 | { 226 | attrs.c_cflag |= (PARENB | PARODD); // enable parity 227 | attrs.c_iflag |= INPCK; 228 | // if(parity == UART_PEVEN) attrs.c_cflag &= ~PARODD; 229 | } 230 | 231 | /* set stop bits */ 232 | attrs.c_cflag &= ~CSTOPB; // 1 stop bit 233 | // attrs.c_cflag |= CSTOPB; // 2 stop bits 234 | 235 | // Disable Hardware flowcontrol 236 | attrs.c_cflag &= ~CRTSCTS; 237 | 238 | /* set to raw mode, disable echo, signals */ 239 | attrs.c_lflag &= ~(ICANON | ECHO | ECHOE | IEXTEN | ISIG); 240 | 241 | /* set no output process, raw mode */ 242 | attrs.c_oflag &= ~OPOST; 243 | attrs.c_oflag &= ~(ONLCR | OCRNL); 244 | 245 | /* disable CR map */ 246 | attrs.c_iflag &= ~(ICRNL | INLCR); 247 | /* disable software flow control */ 248 | attrs.c_iflag &= ~(IXON | IXOFF | IXANY); 249 | 250 | attrs.c_cc[VMIN] = 0; 251 | attrs.c_cc[VTIME] = 0; 252 | 253 | /* flush driver buf */ 254 | tcflush(fd, TCIFLUSH); 255 | 256 | /* update attrs now */ 257 | if (tcsetattr(fd, TCSANOW, &attrs) < 0) 258 | { 259 | close(fd); 260 | // DEBUG("tcsetattr err"); 261 | ERROR((int)SET_COM_ARG_ERR, Error::GetErrorString(SET_COM_ARG_ERR).c_str()); 262 | return -1; 263 | } 264 | 265 | if (change_baud(fd, baudrate)) 266 | { 267 | close(fd); 268 | // DEBUG("fail to set baudrate %d", baudrate); 269 | ERROR((int)CHANGE_BAUDRATE_ERR, Error::GetErrorString(CHANGE_BAUDRATE_ERR).c_str()); 270 | return -1; 271 | } 272 | 273 | return fd; 274 | } 275 | 276 | bool setup_lidar_uart(Parser *hP, int handle) 277 | { 278 | Parser *parser = (Parser *)hP; 279 | unsigned int index = 5; 280 | int cmdLength; 281 | char buf[32]; 282 | 283 | for (unsigned int i = 0; i < index; i++) 284 | { 285 | if ((uart_talk(handle, 6, "LVERSH", 14, "MOTOR VERSION:", 16, buf))) 286 | { 287 | std::string sn = stringfilter(buf, 16); 288 | DEBUG("version:%s\n", sn.c_str()); 289 | break; 290 | } 291 | } 292 | 293 | for (unsigned int i = 0; i < index; i++) 294 | { 295 | cmdLength = strlen(parser->cmd.uuid); 296 | if (cmdLength <= 0) 297 | break; 298 | if ((uart_talk(handle, 6, "LUUIDH", 11, "PRODUCT SN:", 20, buf))) 299 | { 300 | std::string sn = stringfilter(buf, 20); 301 | DEBUG("uuid:%s\n", sn.c_str()); 302 | break; 303 | } 304 | } 305 | for (unsigned int i = 0; i < index; i++) 306 | { 307 | cmdLength = strlen(parser->cmd.model); 308 | if (cmdLength <= 0) 309 | break; 310 | if (uart_talk(handle, cmdLength, parser->cmd.model, 8, "TYPE ID:", 16, buf)) 311 | { 312 | std::string sn = stringfilter(buf, 16); 313 | DEBUG("product model:%s\n", sn.c_str()); 314 | break; 315 | } 316 | } 317 | for (unsigned int i = 0; i < index; i++) 318 | { 319 | cmdLength = strlen(parser->cmd.unit_mm); 320 | if (cmdLength <= 0) 321 | break; 322 | if (uart_talk(handle, cmdLength, parser->cmd.unit_mm, 10, "SET LiDAR ", 12, buf)) 323 | { 324 | DEBUG("set LiDAR unit %s\n", buf); 325 | break; 326 | } 327 | } 328 | // enable/disable output intensity 329 | for (unsigned int i = 0; i < index; i++) 330 | { 331 | cmdLength = strlen(parser->cmd.confidence); 332 | if (cmdLength <= 0) 333 | break; 334 | if (uart_talk(handle, cmdLength, parser->cmd.confidence, 6, "LiDAR ", 12, buf)) 335 | { 336 | DEBUG("set LiDAR confidence %s\n", buf); 337 | break; 338 | } 339 | } 340 | // enable/disable shadow filter 341 | for (unsigned int i = 0; i < index; i++) 342 | { 343 | cmdLength = strlen(parser->cmd.fitter); 344 | if (cmdLength <= 0) 345 | break; 346 | if (uart_talk(handle, cmdLength, parser->cmd.fitter, 2, "ok", 2, buf, 100)) 347 | { 348 | DEBUG("set LiDAR shadow filter ok\n"); 349 | break; 350 | } 351 | } 352 | // enable/disable smooth 353 | 354 | for (unsigned int i = 0; i < index; i++) 355 | { 356 | cmdLength = strlen(parser->cmd.smooth); 357 | if (cmdLength <= 0) 358 | break; 359 | 360 | if (uart_talk(handle, cmdLength, parser->cmd.smooth, 2, "ok", 2, buf, 100)) 361 | { 362 | DEBUG("set LiDAR smooth OK\n"); 363 | break; 364 | } 365 | } 366 | // 设置旋转方向 367 | for (unsigned int i = 0; i < index; i++) 368 | { 369 | cmdLength = strlen(parser->cmd.direction); 370 | if (cmdLength <= 0) 371 | break; 372 | 373 | if (uart_talk(handle, cmdLength, parser->cmd.direction, 2, "OK", 2, buf, 100)) 374 | { 375 | DEBUG("set LiDAR direction OK\n"); 376 | break; 377 | } 378 | } 379 | 380 | // setup rpm 381 | for (unsigned int i = 0; i < index; i++) 382 | { 383 | cmdLength = strlen(parser->cmd.rpm); 384 | if (cmdLength <= 0) 385 | break; 386 | 387 | if (uart_talk(handle, cmdLength, parser->cmd.rpm, 8, "Set RPM:", 12, buf, 100)) 388 | { 389 | DEBUG("%s %s\n", parser->cmd.rpm, buf); 390 | break; 391 | } 392 | } 393 | for (unsigned int i = 0; i < index; i++) 394 | { 395 | cmdLength = strlen(parser->cmd.res); 396 | if (cmdLength <= 0) 397 | break; 398 | if (uart_talk(handle, cmdLength, parser->cmd.res, 15, "set resolution ", 12, buf)) 399 | { 400 | DEBUG("%s %s\n", parser->cmd.res, buf); 401 | break; 402 | } 403 | } 404 | return true; 405 | } 406 | 407 | bool setup_lidar_vpc(Parser *hP, int handle) 408 | { 409 | Parser *parser = (Parser *)hP; 410 | unsigned int index = 5; 411 | int cmdLength; 412 | char buf[32]; 413 | char result[3] = {0}; 414 | result[2] = '\0'; 415 | 416 | for (unsigned int i = 0; i < index; i++) 417 | { 418 | cmdLength = strlen(parser->cmd.ats); 419 | if (cmdLength <= 0) 420 | break; 421 | if (vpc_talk(handle, 0x0053, rand(), cmdLength, parser->cmd.ats, 2, result)) 422 | { 423 | DEBUG("%s %s\n", parser->cmd.ats, result); 424 | break; 425 | } 426 | } 427 | for (unsigned int i = 0; i < index; i++) 428 | { 429 | cmdLength = strlen(parser->cmd.uuid); 430 | if (cmdLength <= 0) 431 | break; 432 | if (vpc_talk(handle, 0x0043, rand(), cmdLength, parser->cmd.uuid, 20, buf)) 433 | { 434 | std::string sn = stringfilter(buf, 20); 435 | DEBUG("uuid:%s\n", sn.c_str()); 436 | break; 437 | } 438 | } 439 | 440 | for (unsigned int i = 0; i < index; i++) 441 | { 442 | cmdLength = strlen(parser->cmd.model); 443 | if (cmdLength <= 0) 444 | break; 445 | if (vpc_talk(handle, 0x0043, rand(), cmdLength, parser->cmd.model, 16, buf)) 446 | { 447 | std::string sn = stringfilter(buf, 16); 448 | DEBUG("product model:%s\n", sn.c_str()); 449 | break; 450 | } 451 | } 452 | 453 | // enable/disable shadow filter 454 | for (unsigned int i = 0; i < index; i++) 455 | { 456 | cmdLength = strlen(parser->cmd.fitter); 457 | if (cmdLength <= 0) 458 | break; 459 | 460 | if (vpc_talk(handle, 0x0053, rand(), cmdLength, parser->cmd.fitter, 2, result)) 461 | { 462 | DEBUG("set LiDAR shadow filter %s %s\n", parser->cmd.fitter, result); 463 | break; 464 | } 465 | } 466 | // enable/disable smooth 467 | for (unsigned int i = 0; i < index; i++) 468 | { 469 | cmdLength = strlen(parser->cmd.smooth); 470 | if (cmdLength <= 0) 471 | break; 472 | 473 | if (vpc_talk(handle, 0x0053, rand(), cmdLength, parser->cmd.smooth, 2, result)) 474 | { 475 | DEBUG("set LiDAR smooth %s %s\n", parser->cmd.smooth, result); 476 | break; 477 | } 478 | } 479 | // setup rpm 480 | for (unsigned int i = 0; i < index; i++) 481 | { 482 | cmdLength = strlen(parser->cmd.rpm); 483 | if (cmdLength <= 0) 484 | break; 485 | 486 | if (vpc_talk(handle, 0x0053, rand(), cmdLength, parser->cmd.rpm, 2, result)) 487 | { 488 | DEBUG("%s %s\n", parser->cmd.rpm, result); 489 | break; 490 | } 491 | } 492 | 493 | for (unsigned int i = 0; i < index; i++) 494 | { 495 | cmdLength = strlen(parser->cmd.res); 496 | if (cmdLength <= 0) 497 | break; 498 | 499 | if (vpc_talk(handle, 0x0053, rand(), cmdLength, parser->cmd.res, 2, result)) 500 | { 501 | DEBUG("%s %s\n", parser->cmd.res, result); 502 | break; 503 | } 504 | } 505 | 506 | // enable/disable alaram message uploading 507 | for (unsigned int i = 0; i < index; i++) 508 | { 509 | cmdLength = strlen(parser->cmd.alarm); 510 | if (cmdLength <= 0) 511 | break; 512 | if (vpc_talk(handle, 0x0053, rand(), cmdLength, parser->cmd.alarm, 2, result)) 513 | { 514 | DEBUG("set alarm_msg %s\n", result); 515 | break; 516 | } 517 | } 518 | 519 | for (unsigned int i = 0; i < index; i++) 520 | { 521 | cmdLength = strlen(parser->cmd.direction); 522 | if (cmdLength <= 0) 523 | break; 524 | if (vpc_talk(handle, 0x0053, rand(), cmdLength, parser->cmd.direction, 2, result)) 525 | { 526 | DEBUG("set direction %s\n", result); 527 | break; 528 | } 529 | } 530 | return true; 531 | } 532 | int UartReader(UartInfo *info) 533 | { 534 | int fd_uart = info->fd_uart; 535 | RawData *fans[MAX_FANS]; 536 | while (1) 537 | { 538 | fd_set fds; 539 | FD_ZERO(&fds); 540 | 541 | FD_SET(fd_uart, &fds); 542 | 543 | struct timeval to = {1, 500000}; 544 | 545 | int ret = select(fd_uart + 1, &fds, NULL, NULL, &to); 546 | if (ret < 0) 547 | { 548 | // printf("select error\n"); 549 | ERROR((int)SOCKET_ERR, Error::GetErrorString(SOCKET_ERR).c_str()); 550 | break; 551 | } 552 | // read UART data 553 | if (FD_ISSET(fd_uart, &fds)) 554 | { 555 | unsigned char buf[512]; 556 | unsigned int nr = read(fd_uart, buf, sizeof(buf)); 557 | if (nr <= 0) 558 | { 559 | //printf("read port error %d\n", nr); 560 | ERROR((int)COM_READ_ERR, Error::GetErrorString(COM_READ_ERR).c_str()); 561 | break; 562 | } 563 | int nfan = ParserRunStream(info->hParser, nr, buf, &(fans[0])); 564 | if (nfan > 0) 565 | { 566 | PublishData(info->hPublish, nfan, fans); 567 | } 568 | if (nr < sizeof(buf) - 10) 569 | usleep(10000 - nr * 10); 570 | } 571 | } 572 | return 0; 573 | } 574 | 575 | void *UartThreadProc(void *p) 576 | { 577 | while (1) 578 | { 579 | UartInfo *info = (UartInfo *)p; 580 | if (access(info->port, R_OK)) 581 | { 582 | ERROR((int)DEVICE_OFFLINE, Error::GetErrorString(DEVICE_OFFLINE).c_str()); 583 | sleep(3); 584 | continue; 585 | } 586 | unsigned int baudrate = 0; 587 | if (info->baudrate <= 0) 588 | { 589 | baudrate = GetComBaud(info->port); 590 | } 591 | else 592 | baudrate = info->baudrate; 593 | 594 | int fd = open_serial_port(info->port, baudrate); 595 | DEBUG("Connection info:uartname %s baudrate %d", info->port, baudrate); 596 | if (fd > 0) 597 | { 598 | info->fd_uart = fd; 599 | if (strcmp(info->type, "uart") == 0) 600 | setup_lidar_uart(info->hParser, info->fd_uart); 601 | if (strcmp(info->type, "vpc") == 0) 602 | setup_lidar_vpc(info->hParser, info->fd_uart); 603 | 604 | UartReader(info); 605 | 606 | } 607 | } 608 | return NULL; 609 | 610 | } 611 | 612 | void *StartUartReader(const char *type, const char *port, int baudrate, Parser *hParser, PubHub *hPublish) 613 | { 614 | UartInfo *info = new UartInfo; 615 | strcpy(info->type, type); 616 | strcpy(info->port, port); 617 | info->baudrate = baudrate; 618 | info->hParser = hParser; 619 | info->hPublish = hPublish; 620 | pthread_create(&info->thr, NULL, UartThreadProc, info); 621 | 622 | return info; 623 | } 624 | 625 | bool SendUartCmd(HReader hr, int len, char *cmd) 626 | { 627 | UartInfo *info = (UartInfo *)hr; 628 | 629 | if (info && info->fd_uart > 0) 630 | write(info->fd_uart, cmd, len); 631 | return true; 632 | } 633 | bool SendVpcCmd(HReader hr, int len, char *cmd) 634 | { 635 | UartInfo *info = (UartInfo *)hr; 636 | if (info && info->fd_uart > 0) 637 | { 638 | int fd = info->fd_uart; 639 | 640 | unsigned char buffer[2048]; 641 | CmdHeader *hdr = (CmdHeader *)buffer; 642 | hdr->sign = 0x484c; 643 | hdr->cmd = 0x0043; 644 | hdr->sn = rand(); 645 | 646 | len = ((len + 3) >> 2) * 4; 647 | 648 | hdr->len = len; 649 | 650 | memcpy(buffer + sizeof(CmdHeader), cmd, len); 651 | 652 | // int n = sizeof(CmdHeader); 653 | unsigned int *pcrc = (unsigned int *)(buffer + sizeof(CmdHeader) + len); 654 | pcrc[0] = stm32crc((unsigned int *)(buffer + 0), len / 4 + 2); 655 | int len2 = len + sizeof(CmdHeader) + 4; 656 | write(fd, buffer, len2); 657 | } 658 | return true; 659 | } 660 | void StopUartReader(HReader hr) 661 | { 662 | UartInfo *info = (UartInfo *)hr; 663 | // info->should_exit = true; 664 | // sleep(1); 665 | pthread_join(info->thr, NULL); 666 | delete info; 667 | } 668 | int GetDevInfoByUART(const char *port_str, unsigned int speed) 669 | { 670 | int zeroNum = 0; 671 | unsigned int check_size = 1024; 672 | int hPort = open_serial_port(port_str, speed); 673 | if (hPort <= 0) 674 | { 675 | return false; 676 | } 677 | char cmd[] = "LVERSH"; 678 | write(hPort, cmd, sizeof(cmd)); 679 | int bOK = false; 680 | unsigned char *buf = new unsigned char[check_size]; 681 | unsigned long rf = 0; 682 | while (rf < check_size) 683 | { 684 | int tmp = read(hPort, buf + rf, check_size - rf); 685 | if (tmp > 0) 686 | { 687 | rf += tmp; 688 | } 689 | else 690 | { 691 | zeroNum++; 692 | usleep(1000 * 10); 693 | } 694 | // if (zeroNum > 10) 695 | // { 696 | // printf("read %d byte max index break\n",rf); 697 | // break; 698 | // } 699 | } 700 | if (zeroNum <= 10) 701 | printf("read max byte %ld break\n", rf); 702 | if (rf > 12) 703 | { 704 | for (unsigned int idx = 0; idx < rf - 12; idx++) 705 | { 706 | if (memcmp((char *)buf + idx, "MCU VERSION:", 12) == 0) 707 | { 708 | bOK = 1; 709 | DEBUG("OK"); 710 | break; 711 | } 712 | } 713 | } 714 | close(hPort); 715 | delete[] buf; 716 | return bOK; 717 | } 718 | 719 | int GetComBaud(std::string uartname) 720 | { 721 | std::vector port_list; 722 | port_list.push_back(230400); 723 | port_list.push_back(256000); 724 | port_list.push_back(384000); 725 | port_list.push_back(460800); 726 | port_list.push_back(500000); 727 | port_list.push_back(768000); 728 | port_list.push_back(921600); 729 | port_list.push_back(1000000); 730 | port_list.push_back(1500000); 731 | 732 | for (unsigned int j = 0; j < port_list.size(); j++) 733 | { 734 | unsigned int com_speed = port_list.at(j); 735 | if (GetDevInfoByUART(uartname.c_str(), com_speed)) 736 | { 737 | return com_speed; 738 | } 739 | } 740 | return 0; 741 | } -------------------------------------------------------------------------------- /bluesea-ros2/src/node.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "rclcpp/clock.hpp" 3 | #include "rclcpp/rclcpp.hpp" 4 | #include "rclcpp/time_source.hpp" 5 | #include "sensor_msgs/msg/laser_scan.hpp" 6 | //#include "sensor_msgs/msg/point_cloud.hpp" 7 | #include "sensor_msgs/msg/point_cloud2.hpp" 8 | //#include "sensor_msgs/point_cloud_conversion.hpp" 9 | #include "base/srv/control.hpp" 10 | #include 11 | #include "../sdk/include/bluesea.h" 12 | 13 | BlueSeaLidarDriver *m_driver = NULL; 14 | void PublishLaserScanFan(rclcpp::Publisher::SharedPtr laser_pub, RawData *fan, std::string &frame_id, double min_dist, double max_dist, uint8_t inverted, uint8_t reversed) 15 | { 16 | (void)inverted; 17 | sensor_msgs::msg::LaserScan msg; 18 | msg.header.stamp.sec = fan->ts[0]; 19 | msg.header.stamp.nanosec = fan->ts[1]; 20 | msg.header.frame_id = frame_id; 21 | 22 | int N = fan->N; 23 | double scan_time = 1 / 100.; 24 | msg.scan_time = scan_time; 25 | msg.time_increment = scan_time / fan->N; 26 | 27 | msg.range_min = min_dist; 28 | msg.range_max = max_dist; // 8.0; 29 | msg.intensities.resize(N); // fan->N); 30 | msg.ranges.resize(N); // fan->N); 31 | 32 | msg.angle_min = m_driver->ROSAng(fan->angle / 10) * 10 * M_PI / 1800; 33 | msg.angle_max = -msg.angle_min; 34 | msg.angle_increment = (fan->span * M_PI / 1800) / fan->N; 35 | 36 | N = 0; 37 | if (reversed) 38 | { 39 | for (int i = fan->N - 1; i >= 0; i--) 40 | { 41 | double d = fan->points[i].distance / 1000.0; 42 | 43 | if (fan->points[i].distance == 0 || d > max_dist || d < min_dist) 44 | msg.ranges[N] = std::numeric_limits::infinity(); 45 | else 46 | msg.ranges[N] = d; 47 | 48 | msg.intensities[N] = fan->points[i].confidence; 49 | N++; 50 | } 51 | } 52 | else 53 | { 54 | for (int i = 0; i < fan->N; i++) 55 | { 56 | double d = fan->points[i].distance / 1000.0; 57 | 58 | if (fan->points[i].distance == 0 || d > max_dist || d < min_dist) 59 | msg.ranges[N] = std::numeric_limits::infinity(); 60 | else 61 | msg.ranges[N] = d; 62 | 63 | msg.intensities[N] = fan->points[i].confidence; 64 | N++; 65 | } 66 | } 67 | laser_pub->publish(msg); 68 | } 69 | void PublishLaserScan(rclcpp::Publisher::SharedPtr laser_pub, HPublish pub, ArgData argdata, uint8_t counterclockwise) 70 | { 71 | PubHub *hub = (PubHub *)pub; 72 | sensor_msgs::msg::LaserScan msg; 73 | int N = hub->consume.size(); 74 | // make min_ang max_ang convert to mask 75 | msg.header.stamp.sec = hub->ts_beg[0]; 76 | msg.header.stamp.nanosec = hub->ts_beg[1]; 77 | 78 | double ti = double(hub->ts_beg[0]) + double(hub->ts_beg[1]) / 1000000000.0; 79 | double tx = double(hub->ts_end[0]) + double(hub->ts_end[1]) / 1000000000.0; 80 | 81 | msg.scan_time = tx - ti; // nfan/(nfan-1); 82 | msg.time_increment = msg.scan_time / N; 83 | 84 | msg.header.frame_id = argdata.frame_id; 85 | 86 | double min_deg = argdata.min_angle * 180 / M_PI; 87 | double max_deg = argdata.max_angle * 180 / M_PI; 88 | 89 | msg.range_min = argdata.min_dist; 90 | msg.range_max = argdata.max_dist; // 8.0; 91 | if (argdata.with_angle_filter) 92 | { 93 | double min_pos, max_pos; 94 | N = m_driver->GetCount(hub->consume, min_deg, max_deg, min_pos, max_pos); 95 | msg.angle_min = min_pos * M_PI / 180; 96 | msg.angle_max = max_pos * M_PI / 180; 97 | msg.angle_increment = (msg.angle_max - msg.angle_min) / (N - 1); 98 | // printf("data1:deg:%lf %lf angle:%lf %lf %d\n",min_deg,max_deg,msg.angle_min,msg.angle_max,N); 99 | } 100 | else 101 | { 102 | if (argdata.from_zero) 103 | { 104 | msg.angle_min = 0; 105 | msg.angle_max = 2 * M_PI * (N - 1) / N; 106 | msg.angle_increment = M_PI * 2 / N; 107 | } 108 | else 109 | { 110 | msg.angle_min = -M_PI; 111 | msg.angle_max = M_PI - (2 * M_PI) / N; 112 | msg.angle_increment = M_PI * 2 / N; 113 | } 114 | } 115 | msg.intensities.resize(N); 116 | msg.ranges.resize(N); 117 | int idx = 0; 118 | // reversed 打开数据倒转顺时针旋转数据也要倒转 119 | if (argdata.reversed + counterclockwise != 1) 120 | { 121 | for (int i = hub->consume.size() - 1; i >= 0; i--) 122 | { 123 | double deg = m_driver->ROSAng(hub->consume[i].degree); 124 | if (argdata.with_angle_filter) 125 | { 126 | if (deg < min_deg) 127 | continue; 128 | if (deg > max_deg) 129 | continue; 130 | } 131 | double d = hub->consume[i].distance / 1000.0; 132 | bool custom = false; 133 | for (unsigned int k = 0; k < argdata.masks.size() && !custom; k++) 134 | { 135 | if (argdata.with_angle_filter && argdata.masks[k].min <= deg && deg <= argdata.masks[k].max) 136 | custom = true; 137 | } 138 | 139 | if (hub->consume[i].distance == 0 || d > argdata.max_dist || d < argdata.min_dist || custom) 140 | msg.ranges[idx] = std::numeric_limits::infinity(); 141 | else 142 | msg.ranges[idx] = d; 143 | 144 | msg.intensities[idx] = hub->consume[i].confidence; 145 | idx++; 146 | } 147 | } 148 | else 149 | { 150 | 151 | // DEBUG("%s %d %f %f\n", __FUNCTION__, __LINE__, hub->consume[0].degree,hub->consume[hub->consume.size()-1].degree); 152 | for (unsigned int i = 0; i <= hub->consume.size() - 1; i++) 153 | { 154 | double deg = m_driver->ROSAng(hub->consume[i].degree); 155 | if (argdata.with_angle_filter) 156 | { 157 | // DEBUG("%lf %lf %lf %lf",hub->consume[i].degree,deg,min_deg,max_deg); 158 | if (deg < min_deg) 159 | continue; 160 | if (deg > max_deg) 161 | continue; 162 | } 163 | double d = hub->consume[i].distance / 1000.0; 164 | bool custom = false; 165 | for (unsigned int k = 0; k < argdata.masks.size() && !custom; k++) 166 | { 167 | if (argdata.with_angle_filter && argdata.masks[k].min <= deg && deg <= argdata.masks[k].max) 168 | custom = true; 169 | } 170 | 171 | if (hub->consume[i].distance == 0 || d > argdata.max_dist || d < argdata.min_dist || custom) 172 | msg.ranges[idx] = std::numeric_limits::infinity(); 173 | else 174 | msg.ranges[idx] = d; 175 | 176 | msg.intensities[idx] = hub->consume[i].confidence; 177 | 178 | idx++; 179 | } 180 | } 181 | laser_pub->publish(msg); 182 | } 183 | 184 | void PublishData(HPublish pub, int n, RawData **fans) 185 | { 186 | int skip = 0; 187 | RawData *drop[MAX_FANS]; 188 | PubHub *hub = (PubHub *)pub; 189 | 190 | pthread_mutex_lock(&hub->mtx); 191 | 192 | if (hub->nfan + n > MAX_FANS) 193 | { 194 | int nr = hub->nfan + n - MAX_FANS; 195 | 196 | for (int i = 0; i < nr; i++) 197 | drop[skip++] = hub->fans[i]; 198 | 199 | for (int i = nr; i < hub->nfan; i++) 200 | { 201 | hub->fans[i - nr] = hub->fans[i]; 202 | } 203 | 204 | hub->nfan -= nr; 205 | } 206 | 207 | for (int i = 0; i < n; i++) 208 | { 209 | hub->fans[hub->nfan++] = fans[i]; 210 | } 211 | pthread_mutex_unlock(&hub->mtx); 212 | for (int i = 0; i < skip; i++) 213 | { 214 | delete drop[i]; 215 | } 216 | } 217 | 218 | #if 0 219 | int angle_cmp(const void* p1, const void* p2) 220 | { 221 | const RawData** pp1 = (const RawData**)p1; 222 | const RawData** pp2 = (const RawData**)p2; 223 | return (*pp1)->ros_angle - (*pp2)->ros_angle; 224 | } 225 | #endif 226 | 227 | bool GetFan(HPublish pub, bool with_resample, double resample_res, RawData **fans) 228 | { 229 | bool got = false; 230 | PubHub *hub = (PubHub *)pub; 231 | if (hub->nfan > 0) 232 | { 233 | pthread_mutex_lock(&hub->mtx); 234 | fans[0] = hub->fans[0]; 235 | for (int i = 1; i < hub->nfan; i++) 236 | hub->fans[i - 1] = hub->fans[i]; 237 | hub->nfan--; 238 | got = true; 239 | pthread_mutex_unlock(&hub->mtx); 240 | } 241 | else 242 | return false; 243 | 244 | if (with_resample) // && resample_res > 0.05) 245 | { 246 | int NN = fans[0]->span / (10 * resample_res); 247 | 248 | if (NN < fans[0]->N) 249 | { 250 | resample(fans[0], NN); 251 | } 252 | else if (NN > fans[0]->N) 253 | { 254 | printf("fan %d less than %d\n", fans[0]->N, NN); 255 | } 256 | } 257 | return got; 258 | } 259 | 260 | struct CloutPointData 261 | { 262 | float x; 263 | float y; 264 | float z; 265 | uint8_t c; 266 | }; 267 | 268 | 269 | void PublishCloud(rclcpp::Publisher::SharedPtr cloud_pub2,HPublish pub, ArgData argdata, uint8_t counterclockwise) 270 | { 271 | PubHub *hub = (PubHub *)pub; 272 | sensor_msgs::msg::PointCloud2 msg; 273 | int N = hub->consume.size(); 274 | msg.header.stamp.sec = hub->ts_beg[0]; 275 | msg.header.stamp.nanosec = hub->ts_beg[1]; 276 | 277 | msg.header.frame_id = argdata.frame_id; 278 | 279 | double min_deg = argdata.min_angle * 180 / M_PI; 280 | double max_deg = argdata.max_angle * 180 / M_PI; 281 | 282 | // if (argdata.with_angle_filter) 283 | // { 284 | // double min_pos, max_pos; 285 | // N = m_driver->GetCount(hub->consume, min_deg, max_deg, min_pos, max_pos); 286 | // } 287 | 288 | msg.height = 1; 289 | msg.width = N; 290 | msg.fields.resize(4); 291 | 292 | msg.fields[0].name = "x"; 293 | msg.fields[0].offset = 0; 294 | msg.fields[0].datatype = sensor_msgs::msg::PointField::FLOAT32; 295 | msg.fields[0].count = 1; 296 | 297 | msg.fields[1].name = "y"; 298 | msg.fields[1].offset = 4; // 假设float是4字节 299 | msg.fields[1].datatype = sensor_msgs::msg::PointField::FLOAT32; 300 | msg.fields[1].count = 1; 301 | 302 | msg.fields[2].name = "z"; 303 | msg.fields[2].offset = 8; 304 | msg.fields[2].datatype = sensor_msgs::msg::PointField::FLOAT32; 305 | msg.fields[2].count = 1; 306 | 307 | msg.fields[3].name = "intensity"; 308 | msg.fields[3].offset = 12; 309 | msg.fields[3].datatype = sensor_msgs::msg::PointField::INT32; 310 | msg.fields[3].count = 1; 311 | 312 | msg.is_bigendian = false; 313 | msg.point_step = 16; // 3个float * 4字节/float 314 | msg.row_step = msg.point_step * msg.width; 315 | msg.data.resize(msg.row_step); 316 | int idx = 0; 317 | 318 | if (argdata.reversed + counterclockwise != 1) 319 | { 320 | for (int i = hub->consume.size() - 1; i >= 0; i--) 321 | { 322 | double deg = m_driver->ROSAng(hub->consume[i].degree); 323 | if (argdata.with_angle_filter) 324 | { 325 | if (deg < min_deg) 326 | continue; 327 | if (deg > max_deg) 328 | continue; 329 | } 330 | double d = hub->consume[i].distance / 1000.0; 331 | bool custom = false; 332 | for (unsigned int k = 0; k < argdata.masks.size() && !custom; k++) 333 | { 334 | if (argdata.with_angle_filter && argdata.masks[k].min <= deg && deg <= argdata.masks[k].max) 335 | custom = true; 336 | } 337 | CloutPointData cpdata; 338 | if (hub->consume[i].distance == 0 || d > argdata.max_dist || d < argdata.min_dist || custom) 339 | { 340 | 341 | cpdata.x =std::numeric_limits::infinity();; 342 | cpdata.y = std::numeric_limits::infinity();; 343 | cpdata.z = std::numeric_limits::infinity();; 344 | cpdata.c= std::numeric_limits::infinity();; 345 | 346 | } 347 | else 348 | { 349 | float a = hub->consume[i].degree * M_PI / 180.0; 350 | cpdata.x = cos(a) * d; 351 | cpdata.y = sin(a) * d * -1; 352 | cpdata.z = 0; 353 | cpdata.c= hub->consume[i].confidence; 354 | } 355 | memcpy(&msg.data[i * msg.point_step], &cpdata, msg.point_step); 356 | //DEBUG("%d %ld",msg.point_step,sizeof(CloutPointData)); 357 | idx++; 358 | 359 | } 360 | } 361 | else 362 | { 363 | 364 | // DEBUG("%s %d %f %f\n", __FUNCTION__, __LINE__, hub->consume[0].degree,hub->consume[hub->consume.size()-1].degree); 365 | for (unsigned int i = 0; i <= hub->consume.size() - 1; i++) 366 | { 367 | double deg = m_driver->ROSAng(hub->consume[i].degree); 368 | if (argdata.with_angle_filter) 369 | { 370 | // DEBUG("%lf %lf %lf %lf",hub->consume[i].degree,deg,min_deg,max_deg); 371 | if (deg < min_deg) 372 | continue; 373 | if (deg > max_deg) 374 | continue; 375 | } 376 | double d = hub->consume[i].distance / 1000.0; 377 | bool custom = false; 378 | for (unsigned int k = 0; k < argdata.masks.size() && !custom; k++) 379 | { 380 | if (argdata.with_angle_filter && argdata.masks[k].min <= deg && deg <= argdata.masks[k].max) 381 | custom = true; 382 | } 383 | 384 | CloutPointData cpdata; 385 | if (hub->consume[i].distance == 0 || d > argdata.max_dist || d < argdata.min_dist || custom) 386 | { 387 | 388 | cpdata.x =std::numeric_limits::infinity();; 389 | cpdata.y = std::numeric_limits::infinity();; 390 | cpdata.z = std::numeric_limits::infinity();; 391 | cpdata.c= std::numeric_limits::infinity();; 392 | 393 | } 394 | else 395 | { 396 | float a = hub->consume[i].degree * M_PI / 180.0; 397 | cpdata.x = cos(a) * d; 398 | cpdata.y = sin(a) * d ; 399 | cpdata.z = 0; 400 | cpdata.c= hub->consume[i].confidence; 401 | } 402 | memcpy(&msg.data[i * msg.point_step], &cpdata, msg.point_step); 403 | //DEBUG("%d %ld",msg.point_step,sizeof(CloutPointData)); 404 | idx++; 405 | } 406 | } 407 | cloud_pub2->publish(msg); 408 | } 409 | 410 | // service call back function 411 | bool control_motor(const std::shared_ptr req, std::shared_ptr res) 412 | { 413 | std::string cmd; 414 | unsigned short proto = 0x0043; 415 | 416 | if (req->func == "stop") 417 | { 418 | cmd = "LSTOPH"; 419 | proto = 0x0043; 420 | } 421 | else if (req->func == "start") 422 | { 423 | cmd = "LSTARH"; 424 | proto = 0x0043; 425 | } 426 | else if (req->func == "switchZone") 427 | { 428 | char tmp[12] = {0}; 429 | sprintf(tmp, "LSAZN:%xH", req->flag); 430 | cmd = tmp; 431 | proto = 0x0053; 432 | 433 | if (req->flag < 0 || req->flag >= 16) 434 | { 435 | res->code = -1; 436 | res->value = "defense zone id is [0,15]"; 437 | return true; 438 | } 439 | } 440 | else if (req->func == "rpm") 441 | { 442 | char tmp[12] = {0}; 443 | sprintf(tmp, "LSRPM:%sH", req->params.c_str()); 444 | cmd = tmp; 445 | proto = 0x0053; 446 | } 447 | else 448 | { 449 | DEBUG("unknown action:%s", req->func.c_str()); 450 | res->code = -1; 451 | res->value = "unknown cmd"; 452 | return true; 453 | } 454 | 455 | DEBUG("action:%s cmd:%s proto:%u", req->func.c_str(), cmd.c_str(), proto); 456 | res->code = m_driver->sendCmd(req->topic, cmd, proto); 457 | if (res->code <= 0) 458 | res->value = "Command delivery failed, check the node terminal printout"; 459 | else if (res->code == 1) 460 | res->value = "OK"; 461 | 462 | return true; 463 | } 464 | 465 | #define READ_PARAM(TYPE, NAME, VAR, INIT) \ 466 | VAR = INIT; \ 467 | node->declare_parameter(NAME, VAR); \ 468 | node->get_parameter(NAME, VAR); 469 | 470 | bool get_range_param(std::shared_ptr node, const char *name, Range &range) 471 | { 472 | std::string mask; 473 | READ_PARAM(std::string, name, mask, ""); 474 | if (mask != "") 475 | { 476 | int index = mask.find(","); 477 | float min = atof(mask.substr(0, index).c_str()); 478 | float max = atof(mask.substr(index + 1).c_str()); 479 | if (min < max) 480 | { 481 | range.min = min * 180 / M_PI; 482 | range.max = max * 180 / M_PI; 483 | return true; 484 | } 485 | } 486 | return false; 487 | } 488 | 489 | bool ProfileInit(std::shared_ptr node, ArgData &argdata) 490 | { 491 | READ_PARAM(int, "number", argdata.num, 1); 492 | READ_PARAM(std::string, "type", argdata.type, "uart"); 493 | READ_PARAM(std::string, "frame_id", argdata.frame_id, "LH_laser"); 494 | READ_PARAM(int, "dev_id", argdata.dev_id, ANYONE); 495 | 496 | printf("argdata.num %d\n", argdata.num); 497 | // dual lidar arg 498 | for (int i = 0; i < argdata.num; i++) 499 | { 500 | ConnectArg arg; 501 | char s[32], t[32]; 502 | if (i == 0) 503 | { 504 | if (argdata.type == "udp") 505 | { 506 | READ_PARAM(std::string, "lidar_ip", arg.arg1, std::string("192.168.158.98")); 507 | READ_PARAM(int, "lidar_port", arg.arg2, 6543); 508 | } 509 | else if (argdata.type == "vpc") 510 | { 511 | READ_PARAM(std::string, "port", arg.arg1, std::string("/dev/ttyACM0")); 512 | READ_PARAM(int, "baud_rate", arg.arg2, 115200); 513 | } 514 | else if (argdata.type == "uart") 515 | { 516 | READ_PARAM(std::string, "port", arg.arg1, std::string("/dev/ttyUSB0")); 517 | READ_PARAM(int, "baud_rate", arg.arg2, 500000); 518 | DEBUG("%d", arg.arg2); 519 | } 520 | else 521 | { 522 | DEBUG("Profiles type=%s is not exist!", argdata.type.c_str()); 523 | return false; 524 | } 525 | 526 | READ_PARAM(std::string, "scan_topic", arg.scan_topics, std::string("scan")); 527 | READ_PARAM(std::string, "cloud_topic", arg.cloud_topics, std::string("cloud")); 528 | } 529 | else 530 | { 531 | if (argdata.type == "udp") 532 | { 533 | // printf("222\n"); 534 | sprintf(s, "lidar%d_ip", i); 535 | READ_PARAM(std::string, s, arg.arg1, std::string("")); 536 | sprintf(s, "lidar%d_port", i); 537 | READ_PARAM(int, s, arg.arg2, 0); 538 | if (arg.arg1.empty() || arg.arg2 == 0) 539 | { 540 | DEBUG("Profiles lidar num %d is not exist!", i); 541 | break; 542 | } 543 | sprintf(s, "scan_topic%d", i); 544 | sprintf(t, "scan%d", i); 545 | READ_PARAM(std::string, s, arg.scan_topics, std::string(t)); 546 | sprintf(s, "cloud_topic%d", i); 547 | sprintf(t, "cloud%d", i); 548 | READ_PARAM(std::string, s, arg.cloud_topics, std::string(t)); 549 | } 550 | } 551 | argdata.connectargs.push_back(arg); 552 | } 553 | READ_PARAM(int, "local_port", argdata.localport, 6668); 554 | // custom 555 | READ_PARAM(bool, "group_listener", argdata.custom.is_group_listener, false); 556 | READ_PARAM(std::string, "group_ip", argdata.custom.group_ip, std::string("224.1.1.91")); 557 | READ_PARAM(int, "raw_bytes", argdata.raw_bytes, 3); // packet mode : 2bytes or 3bytes 558 | 559 | READ_PARAM(bool, "inverted", argdata.inverted, false); 560 | READ_PARAM(bool, "reversed", argdata.reversed, false); 561 | // data output 562 | READ_PARAM(bool, "output_scan", argdata.output_scan, true); // true: enable output angle+distance mode, 0: disable 563 | READ_PARAM(bool, "output_cloud", argdata.output_cloud, false); // false: enable output xyz format, 0 : disable 564 | READ_PARAM(bool, "output_cloud2", argdata.output_cloud2, false); // false: enable output xyz format, 0 : disable 565 | READ_PARAM(bool, "output_360", argdata.output_360, true); // true: packet data of 360 degree (multiple RawData), publish once 566 | // false: publish every RawData (36 degree) 567 | // angle filter 568 | READ_PARAM(bool, "with_angle_filter", argdata.with_angle_filter, false); // true: enable angle filter, false: disable 569 | READ_PARAM(double, "min_angle", argdata.min_angle, -M_PI); // angle filter's low threshold, default value: -pi 570 | READ_PARAM(double, "max_angle", argdata.max_angle, M_PI); // angle filters' up threashold, default value: pi 571 | 572 | // range limitation 573 | READ_PARAM(double, "min_dist", argdata.min_dist, 0.1); // min detection range, default value: 0M 574 | READ_PARAM(double, "max_dist", argdata.max_dist, 50.0); // max detection range, default value: 9999M 575 | // customize angle filter 576 | for (int i = 1;; i++) 577 | { 578 | char name[32]; 579 | sprintf(name, "mask%d", i); 580 | Range range; 581 | if (!get_range_param(node, name, range)) 582 | break; 583 | argdata.masks.push_back(range); 584 | } 585 | READ_PARAM(bool, "from_zero", argdata.from_zero, false); 586 | READ_PARAM(int, "error_circle", argdata.custom.error_circle, 3); 587 | READ_PARAM(double, "error_scale", argdata.custom.error_scale, 0.9); 588 | READ_PARAM(int, "time_mode", argdata.time_mode, 0); 589 | 590 | /*******************************FITTER arg start******************************/ 591 | READ_PARAM(bool, "filter_open", argdata.fitter.isopen, false); 592 | READ_PARAM(int, "filter_type", argdata.fitter.type, 1); 593 | READ_PARAM(double, "max_range", argdata.fitter.max_range, 20.0); 594 | READ_PARAM(double, "min_range", argdata.fitter.min_range, 0.5); 595 | READ_PARAM(double, "max_range_difference", argdata.fitter.max_range_difference, 0.1); 596 | READ_PARAM(int, "filter_window", argdata.fitter.filter_window, 1); 597 | /*******************************FITTER arg end******************************/ 598 | 599 | /*****************************GET arg start************************************/ 600 | READ_PARAM(int, "uuid", argdata.uuid, -1); 601 | // READ_PARAM(int,"model", argdata.model, -1); 602 | /*****************************GET arg end************************************/ 603 | /*****************************SET arg start************************************/ 604 | READ_PARAM(int, "rpm", argdata.rpm, -1); // set motor RPM 605 | // angle composate 606 | READ_PARAM(bool, "hard_resample", argdata.hard_resample, false); // resample angle resolution 607 | READ_PARAM(bool, "soft_resample", argdata.soft_resample, false); // resample angle resolution 608 | READ_PARAM(double, "resample_res", argdata.resample, -1.0); // resample angle resolution 609 | 610 | if (!argdata.hard_resample) 611 | argdata.resample = -1; 612 | 613 | READ_PARAM(int, "with_smooth", argdata.with_smooth, -1); // lidar data smooth filter 614 | READ_PARAM(int, "with_deshadow", argdata.with_deshadow, -1); // data shadow filter 615 | READ_PARAM(int, "alarm_msg", argdata.alarm_msg, -1); // let lidar upload alarm message 616 | READ_PARAM(int, "direction", argdata.direction, -1); 617 | READ_PARAM(int, "unit_is_mm", argdata.unit_is_mm, -1); // 0 : distance is CM, 1: MM 618 | READ_PARAM(int, "with_confidence", argdata.with_confidence, -1); 619 | READ_PARAM(int, "ats", argdata.ats, -1); 620 | /*****************************SET arg end************************************/ 621 | 622 | READ_PARAM(std::string, "ntp_ip", argdata.ntp_ip, "192.168.158.50"); // 0 : distance is CM, 1: MM 623 | READ_PARAM(int, "ntp_port", argdata.ntp_port, 5678); 624 | READ_PARAM(int, "ntp_enable", argdata.ntp_enable, -1); 625 | 626 | READ_PARAM(bool, "log_enable", argdata.log_enable, false); 627 | READ_PARAM(std::string, "log_path", argdata.log_path, "/tmp/ros_log"); 628 | 629 | return true; 630 | } 631 | 632 | int main(int argc, char *argv[]) 633 | { 634 | rclcpp::init(argc, argv); 635 | std::shared_ptr node = rclcpp::Node::make_shared("bluesea_node"); 636 | 637 | rclcpp::QoS qos(0); 638 | qos.reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE); 639 | qos.durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); 640 | // std::shared_ptrptr = rclcpp::Node::make_shared("bluesea_node"); 641 | 642 | ArgData argdata; 643 | ProfileInit(node, argdata); 644 | if(argdata.log_enable) 645 | { 646 | DEBUG("Specific information will be saved to the log file:%s\n", argdata.log_path.c_str()); 647 | redirect_stdout_to_log(argdata.log_path.c_str()); 648 | } 649 | DEBUG("ROS2 VERSION:%s\n", BLUESEA2_VERSION); 650 | m_driver = new BlueSeaLidarDriver(); 651 | m_driver->getInitCmds(argdata); 652 | m_driver->openLidarThread(); 653 | 654 | rclcpp::Publisher::SharedPtr laser_pubs[MAX_LIDARS]; 655 | rclcpp::Publisher::SharedPtr cloud_pubs2[MAX_LIDARS]; 656 | 657 | for (int i = 0; i < argdata.num; i++) 658 | { 659 | if (argdata.output_scan) 660 | laser_pubs[i] = node->create_publisher(argdata.connectargs[i].scan_topics, qos); 661 | if (argdata.output_cloud2) 662 | cloud_pubs2[i] = node->create_publisher(argdata.connectargs[i].cloud_topics, qos); 663 | } 664 | 665 | rclcpp::Service::SharedPtr control_srv[MAX_LIDARS * 2]; 666 | // interacting with the client 667 | for (unsigned int i = 0; i < argdata.connectargs.size(); i++) 668 | { 669 | if (argdata.output_scan) 670 | { 671 | std::string serviceName = argdata.connectargs[i].scan_topics + "_motor"; 672 | control_srv[i] = node->create_service(serviceName, control_motor); 673 | } 674 | if (argdata.output_cloud2) 675 | { 676 | std::string serviceName = argdata.connectargs[i].cloud_topics + "_motor"; 677 | control_srv[i] = node->create_service(serviceName, control_motor); 678 | } 679 | } 680 | rclcpp::WallRate loop_rate(100); 681 | 682 | while (rclcpp::ok()) 683 | { 684 | rclcpp::spin_some(node); 685 | bool idle = true; 686 | for (int i = 0; i < argdata.num; i++) 687 | { 688 | PubHub *hub = m_driver->getHub(i); 689 | if (hub->nfan == 0) 690 | continue; 691 | if (hub->offsetangle == -1) 692 | { 693 | m_driver->checkIsRun(i); 694 | continue; 695 | } 696 | if (!argdata.output_360) 697 | { 698 | RawData *fans[MAX_FANS] = {NULL}; 699 | if (m_driver->GetFan(hub, argdata.soft_resample, argdata.resample, fans)) 700 | { 701 | if (argdata.output_scan) 702 | { 703 | PublishLaserScanFan(laser_pubs[i], fans[0], argdata.frame_id, 704 | argdata.min_dist, argdata.max_dist, argdata.inverted, argdata.reversed); 705 | } 706 | delete fans[0]; 707 | idle = false; 708 | } 709 | } 710 | else 711 | { 712 | // 按照点数判定一帧的数据，然后零数判定，离异点过滤，最大最小值过滤(空点去除)，指定角度屏蔽(mask置零) 713 | int8_t counterclockwise = false; 714 | int n = m_driver->GetAllFans(hub, argdata, counterclockwise); 715 | if (n > 0) 716 | { 717 | idle = false; 718 | if (argdata.output_scan) 719 | { 720 | PublishLaserScan(laser_pubs[i], hub, argdata, counterclockwise); 721 | } 722 | if (argdata.output_cloud2) 723 | { 724 | PublishCloud(cloud_pubs2[i], hub, argdata, counterclockwise); 725 | } 726 | } 727 | hub->consume.clear(); 728 | } 729 | } 730 | if (idle) 731 | { 732 | loop_rate.sleep(); 733 | } 734 | } 735 | return 0; 736 | } -------------------------------------------------------------------------------- /bluesea-ros2/sdk/src/parser.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include "../include/parser.h" 10 | char g_uuid[32] = ""; 11 | char g_model[16] = ""; 12 | int g_timemode = 0; 13 | 14 | void timestampMode(int type) 15 | { 16 | g_timemode = type; 17 | } 18 | static uint32_t update_flags(unsigned char *buf) 19 | { 20 | uint32_t v; 21 | memcpy(&v, buf, 4); 22 | return v; 23 | } 24 | 25 | static unsigned char is_data(unsigned char *buf) 26 | { 27 | if (buf[1] != 0xFA) 28 | return 0; 29 | 30 | if (buf[0] == 0xCE || buf[0] == 0xCF || buf[0] == 0xDF || buf[0] == 0xC7 || buf[0] == 0xAA) 31 | return buf[0]; 32 | 33 | return 0; 34 | } 35 | 36 | static RawData *GetData0xCE_2(const RawDataHdr &hdr, unsigned char *buf, uint32_t flags, bool with_chk) 37 | { 38 | RawData *dat = new RawData; 39 | if (!dat) 40 | { 41 | ERROR((int)MEMORY_SPACE_ERR, Error::GetErrorString(MEMORY_SPACE_ERR).c_str()); 42 | return NULL; 43 | } 44 | memset(dat, 0, sizeof(RawData)); 45 | 46 | memcpy(dat, &hdr, HDR_SIZE); 47 | dat->span = 360; 48 | 49 | int is_mm = getbit(flags, DF_UNIT_IS_MM); 50 | int with_conf = getbit(flags, DF_WITH_INTENSITY); 51 | 52 | // calc checksum 53 | unsigned short sum = hdr.angle + hdr.N, chk; 54 | unsigned char *pdat = buf + HDR_SIZE; 55 | for (int i = 0; i < hdr.N; i++) 56 | { 57 | unsigned short v = *pdat++; 58 | unsigned short v2 = *pdat++; 59 | unsigned short val = (v2 << 8) | v; 60 | if (with_conf) 61 | { 62 | dat->points[i].confidence = val >> 13; 63 | dat->points[i].distance = val & 0x1fff; 64 | // DEBUG("%d %d",is_mm,dat->points[i].distance); 65 | // if(val&0x7) 66 | // DEBUG("%d",val&0x7); 67 | 68 | if (is_mm == 0) 69 | dat->points[i].distance *= 10; 70 | } 71 | else 72 | { 73 | dat->points[i].distance = is_mm ? val : val * 10; 74 | dat->points[i].confidence = 0; 75 | } 76 | 77 | // dat->points[i].angle = hdr.angle*10 + 3600 * i / hdr.N; 78 | dat->points[i].degree = hdr.angle / 10.0 + (dat->span * i) / (10.0 * hdr.N); 79 | 80 | sum += val; 81 | } 82 | 83 | memcpy(&chk, buf + HDR_SIZE + hdr.N * 2, 2); 84 | 85 | if (with_chk && chk != sum) 86 | { 87 | delete dat; 88 | // ERROR((int)CHECKSUM_ERR, Error::GetErrorString(CHECKSUM_ERR).c_str()); 89 | printf("chksum ce 2 error\n"); 90 | return NULL; 91 | } 92 | SetTimeStamp(dat); 93 | return dat; 94 | } 95 | 96 | static RawData *GetData0xCE_3(const RawDataHdr &hdr, unsigned char *buf, uint32_t flags, bool with_chk) 97 | { 98 | RawData *dat = new RawData; 99 | if (!dat) 100 | { 101 | printf("out of memory\n"); 102 | return NULL; 103 | } 104 | memset(dat, 0, sizeof(RawData)); 105 | 106 | memcpy(dat, &hdr, HDR_SIZE); 107 | int span = (flags & DF_FAN_90) ? 90 : 360; 108 | 109 | if (hdr.angle == 3420 && (flags & DF_FAN_90)) 110 | span = 180; 111 | dat->span = span; 112 | 113 | unsigned char *pdat = buf + HDR_SIZE; 114 | // calc checksum 115 | unsigned short sum = hdr.angle + hdr.N, chk; 116 | for (int i = 0; i < hdr.N; i++) 117 | { 118 | dat->points[i].confidence = *pdat++; 119 | sum += dat->points[i].confidence; 120 | 121 | unsigned short v = *pdat++; 122 | unsigned short v2 = *pdat++; 123 | unsigned short vv = (v2 << 8) | v; 124 | sum += vv; 125 | dat->points[i].distance = vv; 126 | // dat->points[i].angle = hdr.angle*10 + span * i * 10 / hdr.N; 127 | dat->points[i].degree = hdr.angle / 10.0 + (span * i) / (10.0 * hdr.N); 128 | } 129 | 130 | memcpy(&chk, pdat, 2); 131 | 132 | if (with_chk && chk != sum) 133 | { 134 | delete dat; 135 | printf("chksum ce 3 error\n"); 136 | return NULL; 137 | } 138 | 139 | // memcpy(dat.data, buf+idx+HDR_SIZE, 2*hdr.N); 140 | // printf("get3 %d(%d)\n", hdr.angle, hdr.N); 141 | 142 | SetTimeStamp(dat); 143 | return dat; 144 | } 145 | 146 | static FanSegment_C7 *GetFanSegment(const RawDataHdr7 &hdr, uint8_t *pdat, bool /*with_chk*/) 147 | { 148 | FanSegment_C7 *fan_seg = new FanSegment_C7; 149 | if (!fan_seg) 150 | { 151 | printf("out of memory\n"); 152 | return NULL; 153 | } 154 | fan_seg->hdr = hdr; 155 | fan_seg->next = NULL; 156 | 157 | uint16_t sum = 0; 158 | // if (with_chk) 159 | { 160 | uint16_t *pchk = (uint16_t *)pdat; 161 | for (int i = 1; i < HDR7_SIZE / 2; i++) 162 | sum += pchk[i]; 163 | } 164 | 165 | uint8_t *pDist = pdat + HDR7_SIZE; 166 | uint8_t *pAngle = pdat + HDR7_SIZE + 2 * hdr.N; 167 | uint8_t *energy = pdat + HDR7_SIZE + 4 * hdr.N; 168 | 169 | for (int i = 0; i < hdr.N; i++, pDist += 2, pAngle += 2) 170 | { 171 | fan_seg->dist[i] = ((uint16_t)(pDist[1]) << 8) | pDist[0]; 172 | fan_seg->angle[i] = ((uint16_t)(pAngle[1]) << 8) | pAngle[0]; 173 | fan_seg->energy[i] = energy[i]; 174 | 175 | sum += fan_seg->dist[i]; 176 | sum += fan_seg->angle[i]; 177 | sum += energy[i]; 178 | } 179 | 180 | uint8_t *pchk = pdat + HDR7_SIZE + 5 * hdr.N; 181 | uint16_t chksum = ((uint16_t)(pchk[1]) << 8) | pchk[0]; 182 | if (chksum != sum) 183 | { 184 | printf("checksum error\n"); 185 | delete fan_seg; 186 | return NULL; 187 | } 188 | return fan_seg; 189 | } 190 | static FanSegment_AA *GetFanSegment(const RawDataHdrAA &hdr, uint8_t *pdat, bool /*with_chk*/) 191 | { 192 | FanSegment_AA *fan_seg = new FanSegment_AA; 193 | if (!fan_seg) 194 | { 195 | printf("out of memory\n"); 196 | return NULL; 197 | } 198 | fan_seg->hdr = hdr; 199 | fan_seg->next = NULL; 200 | 201 | uint16_t sum = 0; 202 | // if (with_chk) 203 | { 204 | uint16_t *pchk = (uint16_t *)pdat; 205 | for (int i = 1; i < HDRAA_SIZE / 2; i++) 206 | sum += pchk[i]; 207 | } 208 | 209 | uint8_t *pDist = pdat + HDRAA_SIZE; 210 | uint8_t *pAngle = pdat + HDRAA_SIZE + 2 * hdr.N; 211 | uint8_t *energy = pdat + HDRAA_SIZE + 4 * hdr.N; 212 | 213 | for (int i = 0; i < hdr.N; i++, pDist += 2, pAngle += 2) 214 | { 215 | fan_seg->dist[i] = ((uint16_t)(pDist[1]) << 8) | pDist[0]; 216 | fan_seg->angle[i] = ((uint16_t)(pAngle[1]) << 8) | pAngle[0]; 217 | fan_seg->energy[i] = energy[i]; 218 | 219 | sum += fan_seg->dist[i]; 220 | sum += fan_seg->angle[i]; 221 | sum += energy[i]; 222 | } 223 | 224 | uint8_t *pchk = pdat + HDRAA_SIZE + 5 * hdr.N; 225 | uint16_t chksum = ((uint16_t)(pchk[1]) << 8) | pchk[0]; 226 | if (chksum != sum) 227 | { 228 | printf("checksum error\n"); 229 | delete fan_seg; 230 | return NULL; 231 | } 232 | return fan_seg; 233 | } 234 | void DecTimestamp(uint32_t ts, uint32_t *ts2) 235 | { 236 | timeval tv; 237 | gettimeofday(&tv, NULL); 238 | uint32_t sec = tv.tv_sec % 3600; 239 | if (sec < 5 && ts / 1000 > 3595) 240 | { 241 | ts2[0] = (tv.tv_sec / 3600 - 1) * 3600 + ts / 1000; 242 | } 243 | else 244 | { 245 | ts2[0] = (tv.tv_sec / 3600) * 3600 + ts / 1000; 246 | } 247 | 248 | ts2[1] = (ts % 1000) * 1000000; 249 | } 250 | 251 | static RawData *PackFanData(FanSegment_C7 *seg) 252 | { 253 | RawData *dat = new RawData; 254 | if (!dat) 255 | { 256 | printf("out of memory\n"); 257 | return NULL; 258 | } 259 | memset(dat, 0, sizeof(RawData)); 260 | 261 | dat->code = 0xfac7; 262 | dat->N = seg->hdr.whole_fan; 263 | dat->angle = seg->hdr.beg_ang / 100; // 0.1 degree 264 | dat->span = (seg->hdr.end_ang - seg->hdr.beg_ang) / 100; // 0.1 degree 265 | dat->fbase = 0; 266 | dat->first = 0; 267 | dat->last = 0; 268 | dat->fend = 0; 269 | dat->counterclockwise = (seg->hdr.flags & DF_MOTOR_REVERSE) ? 1 : 0; 270 | 271 | if (!g_timemode) 272 | SetTimeStamp(dat); 273 | else 274 | DecTimestamp(seg->hdr.timestamp, dat->ts); 275 | int count = 0; 276 | while (seg) 277 | { 278 | for (int i = 0; i < seg->hdr.N; i++, count++) 279 | { 280 | dat->points[count].confidence = seg->energy[i]; 281 | dat->points[count].distance = seg->dist[i]; 282 | dat->points[count].degree = (seg->angle[i] + seg->hdr.beg_ang) / 1000.0; 283 | } 284 | 285 | seg = seg->next; 286 | } 287 | 288 | return dat; 289 | } 290 | static RawData *PackFanData(FanSegment_AA *seg) 291 | { 292 | RawData *dat = new RawData; 293 | if (!dat) 294 | { 295 | printf("out of memory\n"); 296 | return NULL; 297 | } 298 | memset(dat, 0, sizeof(RawData)); 299 | 300 | dat->code = 0xfaaa; 301 | dat->N = seg->hdr.whole_fan; 302 | dat->angle = seg->hdr.beg_ang / 100; // 0.1 degree 303 | dat->span = (seg->hdr.end_ang - seg->hdr.beg_ang) / 100; // 0.1 degree 304 | dat->fbase = 0; 305 | dat->first = 0; 306 | dat->last = 0; 307 | dat->fend = 0; 308 | dat->counterclockwise = (seg->hdr.flags & DF_MOTOR_REVERSE) ? 1 : 0; 309 | 310 | if (!g_timemode) 311 | SetTimeStamp(dat); 312 | else 313 | { 314 | if(seg->hdr.second>2208988800U) 315 | dat->ts[0] = seg->hdr.second-2208988800U; 316 | else 317 | dat->ts[0] = seg->hdr.second; 318 | dat->ts[1] = seg->hdr.nano_sec; 319 | } 320 | int count = 0; 321 | while (seg) 322 | { 323 | for (int i = 0; i < seg->hdr.N; i++, count++) 324 | { 325 | dat->points[count].confidence = seg->energy[i]; 326 | dat->points[count].distance = seg->dist[i]; 327 | dat->points[count].degree = (seg->angle[i] + seg->hdr.beg_ang) / 1000.0; 328 | } 329 | 330 | seg = seg->next; 331 | } 332 | 333 | return dat; 334 | } 335 | static int GetFanPointCount(FanSegment_C7 *seg) 336 | { 337 | int n = 0; 338 | 339 | while (seg) 340 | { 341 | n += seg->hdr.N; 342 | seg = seg->next; 343 | } 344 | 345 | return n; 346 | } 347 | static int GetFanPointCount(FanSegment_AA *seg) 348 | { 349 | int n = 0; 350 | 351 | while (seg) 352 | { 353 | n += seg->hdr.N; 354 | seg = seg->next; 355 | } 356 | 357 | return n; 358 | } 359 | static RawData *GetData0xC7(Parser *parser, const RawDataHdr7 &hdr, uint8_t *pdat) 360 | { 361 | if (parser->dev_id != (u_int32_t)ANYONE && hdr.dev_id != parser->dev_id) 362 | { 363 | static time_t last = 0; 364 | time_t t = time(NULL); 365 | if (t > last) 366 | { 367 | printf("device id [%d] != my id [%d]\n", hdr.dev_id, parser->dev_id); 368 | last = t; 369 | } 370 | // not my data 371 | return NULL; 372 | } 373 | 374 | FanSegment_C7 *fan_seg = GetFanSegment(hdr, pdat, parser->with_chk); 375 | if (!fan_seg) 376 | { 377 | return NULL; 378 | } 379 | 380 | // printf("fan %d %d\n", fan_seg->hdr.beg_ang, fan_seg->hdr.ofset); 381 | 382 | if (parser->fan_segs_c7 != NULL) 383 | { 384 | FanSegment_C7 *seg = parser->fan_segs_c7; 385 | 386 | if (seg->hdr.timestamp != fan_seg->hdr.timestamp) 387 | { 388 | ERROR((int)SUBPACKET_MERGE_ERROR, Error::GetErrorString(SUBPACKET_MERGE_ERROR).c_str()); 389 | while (seg) 390 | { 391 | parser->fan_segs_c7 = seg->next; 392 | delete seg; 393 | seg = parser->fan_segs_c7; 394 | } 395 | parser->fan_segs_c7 = fan_seg; 396 | } 397 | else 398 | { 399 | while (seg) 400 | { 401 | if (seg->hdr.ofset == fan_seg->hdr.ofset) 402 | { 403 | ERROR((int)SUBPACKET_REPEAT, Error::GetErrorString(SUBPACKET_REPEAT).c_str()); 404 | delete fan_seg; 405 | fan_seg = NULL; 406 | break; 407 | } 408 | if (seg->next == NULL) 409 | { 410 | seg->next = fan_seg; 411 | break; 412 | } 413 | seg = seg->next; 414 | } 415 | } 416 | } 417 | 418 | if (parser->fan_segs_c7 == NULL && fan_seg != NULL) 419 | { 420 | parser->fan_segs_c7 = fan_seg; 421 | } 422 | 423 | // if (parser->fan_segs == NULL) { return NULL; } 424 | 425 | unsigned int N = GetFanPointCount(parser->fan_segs_c7); 426 | 427 | if (N >= parser->fan_segs_c7->hdr.whole_fan) 428 | { 429 | RawData *dat = NULL; 430 | 431 | if (N == parser->fan_segs_c7->hdr.whole_fan) 432 | { 433 | if (N > sizeof(dat->points) / sizeof(dat->points[0])) 434 | { 435 | printf("too many %d points in 1 fan\n", N); 436 | } 437 | else 438 | { 439 | dat = PackFanData(parser->fan_segs_c7); 440 | } 441 | } 442 | 443 | // remove segments 444 | FanSegment_C7 *seg = parser->fan_segs_c7; 445 | while (seg) 446 | { 447 | parser->fan_segs_c7 = seg->next; 448 | delete seg; 449 | seg = parser->fan_segs_c7; 450 | } 451 | 452 | if (dat) 453 | { 454 | // SetTimeStamp(dat, ); 455 | // dat->ros_angle = LidarAng2ROS(dat->angle + dat->span); 456 | } 457 | 458 | return dat; 459 | } 460 | return NULL; 461 | } 462 | 463 | static RawData *GetData0xAA(Parser *parser, const RawDataHdrAA &hdr, uint8_t *pdat) 464 | { 465 | if (parser->dev_id != (u_int32_t)ANYONE && hdr.dev_id != parser->dev_id) 466 | { 467 | static time_t last = 0; 468 | time_t t = time(NULL); 469 | if (t > last) 470 | { 471 | printf("device id [%d] != my id [%d]\n", hdr.dev_id, parser->dev_id); 472 | last = t; 473 | } 474 | // not my data 475 | return NULL; 476 | } 477 | FanSegment_AA *fan_seg = GetFanSegment(hdr, pdat, parser->with_chk); 478 | if (!fan_seg) 479 | { 480 | return NULL; 481 | } 482 | 483 | // printf("fan %d %d\n", fan_seg->hdr.beg_ang, fan_seg->hdr.ofset); 484 | 485 | if (parser->fan_segs_aa != NULL) 486 | { 487 | FanSegment_AA *seg = parser->fan_segs_aa; 488 | 489 | if ((seg->hdr.second != fan_seg->hdr.second) || (seg->hdr.nano_sec != fan_seg->hdr.nano_sec)) 490 | { 491 | printf("drop old fan segments\n"); 492 | while (seg) 493 | { 494 | parser->fan_segs_aa = seg->next; 495 | delete seg; 496 | seg = parser->fan_segs_aa; 497 | } 498 | parser->fan_segs_aa = fan_seg; 499 | } 500 | else 501 | { 502 | while (seg) 503 | { 504 | if (seg->hdr.ofset == fan_seg->hdr.ofset) 505 | { 506 | printf("drop duplicated segment\n"); 507 | delete fan_seg; 508 | fan_seg = NULL; 509 | break; 510 | } 511 | if (seg->next == NULL) 512 | { 513 | seg->next = fan_seg; 514 | break; 515 | } 516 | seg = seg->next; 517 | } 518 | } 519 | } 520 | 521 | if (parser->fan_segs_aa == NULL && fan_seg != NULL) 522 | { 523 | parser->fan_segs_aa = fan_seg; 524 | } 525 | 526 | // if (parser->fan_segs == NULL) { return NULL; } 527 | 528 | unsigned int N = GetFanPointCount(parser->fan_segs_aa); 529 | 530 | if (N >= parser->fan_segs_aa->hdr.whole_fan) 531 | { 532 | RawData *dat = NULL; 533 | 534 | if (N == parser->fan_segs_aa->hdr.whole_fan) 535 | { 536 | if (N > sizeof(dat->points) / sizeof(dat->points[0])) 537 | { 538 | printf("too many %d points in 1 fan\n", N); 539 | } 540 | else 541 | { 542 | dat = PackFanData(parser->fan_segs_aa); 543 | } 544 | } 545 | 546 | // remove segments 547 | FanSegment_AA *seg = parser->fan_segs_aa; 548 | while (seg) 549 | { 550 | parser->fan_segs_aa = seg->next; 551 | delete seg; 552 | seg = parser->fan_segs_aa; 553 | } 554 | 555 | if (dat) 556 | { 557 | // SetTimeStamp(dat, ); 558 | // dat->ros_angle = LidarAng2ROS(dat->angle + dat->span); 559 | } 560 | 561 | return dat; 562 | } 563 | 564 | return NULL; 565 | } 566 | 567 | static RawData *GetData0x99(const RawDataHdr99 &hdr, unsigned char *pdat, bool) 568 | { 569 | RawData *dat = new RawData; 570 | 571 | if (!dat) 572 | { 573 | printf("out of memory\n"); 574 | return NULL; 575 | } 576 | 577 | memset(dat, 0, sizeof(RawData)); 578 | dat->code = hdr.code; 579 | dat->N = hdr.N; 580 | dat->angle = hdr.from * 3600 / hdr.total; // 0.1 degree 581 | dat->span = hdr.N * 3600 / hdr.total; // 0.1 degree 582 | // dat->fbase = ; 583 | // dat->first; 584 | // dat->last; 585 | // dat->fend; 586 | if (!g_timemode) 587 | SetTimeStamp(dat); 588 | else 589 | DecTimestamp(hdr.timestamp, dat->ts); 590 | 591 | dat->counterclockwise = (hdr.flags & DF_MOTOR_REVERSE) ? 1 : 0; 592 | 593 | pdat += HDR99_SIZE; 594 | 595 | uint8_t *dist = pdat; 596 | uint8_t *energy = pdat + 2 * hdr.N; 597 | 598 | for (int i = 0; i < hdr.N; i++) 599 | { 600 | uint16_t lo = *dist++; 601 | uint16_t hi = *dist++; 602 | dat->points[i].distance = (hi << 8) | lo; 603 | dat->points[i].degree = (i + hdr.from) * 360.0 / hdr.total; 604 | dat->points[i].confidence = energy[i]; 605 | } 606 | 607 | return dat; 608 | } 609 | 610 | static RawData *GetData0xCF(const RawDataHdr2 &hdr, unsigned char *pdat, bool with_chk) 611 | { 612 | RawData *dat = new RawData; 613 | if (!dat) 614 | { 615 | printf("out of memory\n"); 616 | return NULL; 617 | } 618 | memset(dat, 0, sizeof(RawData)); 619 | 620 | memcpy(dat, &hdr, HDR2_SIZE); 621 | 622 | unsigned short sum = hdr.angle + hdr.N + hdr.span, chk; 623 | 624 | pdat += HDR2_SIZE; 625 | 626 | for (int i = 0; i < hdr.N; i++) 627 | { 628 | dat->points[i].confidence = *pdat++; 629 | sum += dat->points[i].confidence; 630 | 631 | unsigned short v = *pdat++; 632 | unsigned short v2 = *pdat++; 633 | 634 | unsigned short vv = (v2 << 8) | v; 635 | 636 | sum += vv; 637 | dat->points[i].distance = vv; 638 | // dat->points[i].angle = hdr.angle*10 + hdr.span * i *10 / hdr.N; 639 | dat->points[i].degree = hdr.angle / 10.0 + (hdr.span * i) / (10.0 * hdr.N); 640 | } 641 | 642 | memcpy(&chk, pdat, 2); 643 | 644 | if (with_chk && chk != sum) 645 | { 646 | delete dat; 647 | printf("chksum cf error"); 648 | return NULL; 649 | } 650 | 651 | // memcpy(dat.data, buf+idx+HDR_SIZE, 2*hdr.N); 652 | // printf("get CF %d(%d) %d\n", hdr.angle, hdr.N, hdr.span); 653 | 654 | SetTimeStamp(dat); 655 | dat->counterclockwise = -1; 656 | return dat; 657 | } 658 | 659 | static RawData *GetData0xDF(const RawDataHdr3 &hdr, unsigned char *pdat, bool with_chk) 660 | { 661 | RawData *dat = new RawData; 662 | 663 | if (!dat) 664 | { 665 | printf("out of memory\n"); 666 | return NULL; 667 | } 668 | memset(dat, 0, sizeof(RawData)); 669 | 670 | memcpy(dat, &hdr, HDR3_SIZE); 671 | 672 | unsigned short sum = hdr.angle + hdr.N + hdr.span, chk; 673 | 674 | sum += hdr.fbase; 675 | sum += hdr.first; 676 | sum += hdr.last; 677 | sum += hdr.fend; 678 | 679 | double dan = (hdr.last - hdr.first) / double(hdr.N - 1); 680 | 681 | pdat += HDR3_SIZE; 682 | 683 | for (int i = 0; i < hdr.N; i++) 684 | { 685 | dat->points[i].confidence = *pdat++; 686 | sum += dat->points[i].confidence; 687 | 688 | unsigned short v = *pdat++; 689 | unsigned short v2 = *pdat++; 690 | unsigned short vv = (v2 << 8) | v; 691 | sum += vv; 692 | dat->points[i].distance = vv; 693 | // dat->points[i].angle = hdr.first + dan * i; 694 | dat->points[i].degree = (hdr.first + dan * i) / 100.0; 695 | } 696 | 697 | memcpy(&chk, pdat, 2); 698 | 699 | if (with_chk && chk != sum) 700 | { 701 | delete dat; 702 | printf("chksum df error"); 703 | return NULL; 704 | } 705 | 706 | // memcpy(dat.data, buf+idx+HDR_SIZE, 2*hdr.N); 707 | // printf("get DF %d=%d %d %d\n", hdr.angle, hdr.first, hdr.N, hdr.span); 708 | 709 | SetTimeStamp(dat); 710 | dat->counterclockwise = -1; 711 | return dat; 712 | } 713 | 714 | static int MsgProc(Parser *parser, int len, unsigned char *buf) 715 | { 716 | if (len >= 8 && buf[0] == 'S' && buf[1] == 'T' && buf[6] == 'E' && buf[7] == 'D') 717 | { 718 | parser->flags = update_flags(buf + 2); 719 | return 1; 720 | } 721 | else if (alarmProc(buf, len)) 722 | { 723 | return 2; 724 | } 725 | else if (len >= 4 && buf[0] == 0xce && buf[1] == 0xce) 726 | { 727 | if (buf[2] == 0x0 && buf[3] == 0x01) 728 | { 729 | ERROR(ALARM_LOWPOWER, Error::GetErrorString(ALARM_LOWPOWER).c_str()); 730 | } 731 | else if (buf[2] == 0x0 && buf[3] == 0x02) 732 | { 733 | ERROR(ALARM_ZERO_ERR, Error::GetErrorString(ALARM_ZERO_ERR).c_str()); 734 | } 735 | else if (buf[2] == 0x0 && buf[3] == 0x03) 736 | { 737 | ERROR(ALARM_CODEDISK_ERR, Error::GetErrorString(ALARM_CODEDISK_ERR).c_str()); 738 | } 739 | else if (buf[2] == 0x0 && buf[3] == 0x04) 740 | { 741 | ERROR(ALARM_RESET_ERR, Error::GetErrorString(ALARM_RESET_ERR).c_str()); 742 | } 743 | else if (buf[2] == 0x0 && buf[3] == 0x05) 744 | { 745 | ERROR(ALARM_MOUDLE_ERR, Error::GetErrorString(ALARM_MOUDLE_ERR).c_str()); 746 | } 747 | return 3; 748 | } 749 | else 750 | { 751 | printf("unknown %d bytes : %02x ", len, buf[0]); 752 | for (int i = 1; i < len; i++) 753 | printf("%02x ", buf[i]); 754 | printf("\n"); 755 | } 756 | 757 | return -1; 758 | } 759 | 760 | static int ParseStream(Parser *parser, int len, unsigned char *buf, int *nfan, RawData *fans[]) 761 | { 762 | int max_fan = *nfan; 763 | int idx = 0; 764 | *nfan = 0; 765 | int unk = 0; 766 | unsigned char unknown[1024]; 767 | while (idx < len - 128 && *nfan < max_fan) 768 | { 769 | unsigned char type = is_data(buf + idx); 770 | if (type == 0) 771 | { 772 | unknown[unk++] = buf[idx++]; 773 | if (unk == sizeof(unknown)) 774 | { 775 | printf("drop %d bytes : %02x %02x %02x %02x ...\n", unk, 776 | unknown[0], unknown[1], 777 | unknown[2], unknown[3]); 778 | unk = 0; 779 | } 780 | continue; 781 | } 782 | 783 | if (unk > 0) 784 | { 785 | int ret = MsgProc(parser, unk, unknown); 786 | if (ret == 1) 787 | { 788 | int8_t flag = parser->flags >> 24; 789 | if (strcmp(g_model, "LDS-50C-R") == 0 || strcmp(g_model, "LDS-E200-R") == 0 || strcmp(g_model, "LDS-E200-A") == 0 || strcmp(g_model, "LDS-E200-A4") == 0 790 | || strcmp(g_model, "LDS-E210-R") == 0|| strcmp(g_model, "LDS-E210-A") == 0) 791 | { 792 | if (flag & 0x1) 793 | { 794 | ERROR((int)ALARM_BOTTOMPLATE_LOW_VOLTAGE, Error::GetErrorString(ALARM_BOTTOMPLATE_LOW_VOLTAGE).c_str()); 795 | } 796 | if (flag & 0x2) 797 | { 798 | ERROR((int)ALARM_BOTTOMPLATE_HIGH_VOLTAGE, Error::GetErrorString(ALARM_BOTTOMPLATE_HIGH_VOLTAGE).c_str()); 799 | } 800 | if (flag & 0x4) 801 | { 802 | ERROR((int)ALARM_TEMPERATURE_ERR, Error::GetErrorString(ALARM_TEMPERATURE_ERR).c_str()); 803 | } 804 | if (flag & 0x8) 805 | { 806 | ERROR((int)ALARM_MOTERHEAD_LOW_VOLTAGE, Error::GetErrorString(ALARM_MOTERHEAD_LOW_VOLTAGE).c_str()); 807 | } 808 | if (flag & 0x10) 809 | { 810 | ERROR((int)ALARM_MOTERHEAD_HIGH_VOLTAGE, Error::GetErrorString(ALARM_MOTERHEAD_HIGH_VOLTAGE).c_str()); 811 | } 812 | } 813 | } 814 | else if (ret == -1) 815 | { 816 | parser->rest_len = 0; 817 | memset(parser->rest_buf, 0, sizeof(parser->rest_buf)); 818 | } 819 | unk = 0; 820 | } 821 | 822 | RawDataHdr hdr; 823 | memcpy(&hdr, buf + idx, HDR_SIZE); 824 | 825 | if (hdr.N > MAX_POINTS) 826 | { 827 | ERROR((int)POINT_NUM_LARGE, Error::GetErrorString(POINT_NUM_LARGE).c_str()); 828 | idx += 2; 829 | continue; 830 | } 831 | 832 | if (type == 0xCE) 833 | { 834 | int raw_mode = 3; 835 | if (idx + HDR_SIZE + hdr.N * 3 + 2 > len) 836 | break; 837 | 838 | if (parser->raw_mode == 3) 839 | { 840 | RawData *fan = GetData0xCE_3(hdr, buf + idx, parser->flags, parser->with_chk); 841 | if (fan) 842 | { 843 | fans[*nfan] = fan; 844 | *nfan += 1; 845 | idx += HDR_SIZE + hdr.N * 3 + 2; 846 | fan->counterclockwise = -1; 847 | } 848 | else 849 | { 850 | if (!parser->isrun) 851 | { 852 | fan = GetData0xCE_2(hdr, buf + idx, parser->flags, parser->with_chk); 853 | if (fan) 854 | { 855 | fans[*nfan] = fan; 856 | *nfan += 1; 857 | raw_mode = 2; 858 | idx += HDR_SIZE + hdr.N * 2 + 2; 859 | fan->counterclockwise = 1; 860 | } 861 | else 862 | idx += HDR_SIZE + hdr.N * 3 + 2; 863 | } 864 | else 865 | idx += HDR_SIZE + hdr.N * 3 + 2; 866 | } 867 | } 868 | else 869 | { 870 | RawData *fan = GetData0xCE_2(hdr, buf + idx, parser->flags, parser->with_chk); 871 | if (fan) 872 | { 873 | fans[*nfan] = fan; 874 | *nfan += 1; 875 | idx += HDR_SIZE + hdr.N * 2 + 2; 876 | raw_mode = 2; 877 | fan->counterclockwise = 1; 878 | } 879 | else 880 | { 881 | if (!parser->isrun) 882 | { 883 | fan = GetData0xCE_3(hdr, buf + idx, parser->flags, parser->with_chk); 884 | if (fan) 885 | { 886 | fans[*nfan] = fan; 887 | *nfan += 1; 888 | idx += HDR_SIZE + hdr.N * 3 + 2; 889 | fan->counterclockwise = -1; 890 | } 891 | else 892 | idx += HDR_SIZE + hdr.N * 2 + 2; 893 | } 894 | else 895 | idx += HDR_SIZE + hdr.N * 2 + 2; 896 | } 897 | } 898 | if (!parser->isrun) 899 | parser->raw_mode = raw_mode; 900 | } 901 | else if (type == 0x99) 902 | { 903 | if (idx + hdr.N * 3 + HDR99_SIZE + 2 > len) 904 | { 905 | // need more bytes 906 | break; 907 | } 908 | 909 | RawDataHdr99 hdr99; 910 | memcpy(&hdr99, buf + idx, HDR99_SIZE); 911 | 912 | RawData *fan = GetData0x99(hdr99, buf + idx, parser->with_chk); 913 | if (fan) 914 | { 915 | fans[*nfan] = fan; 916 | *nfan += 1; 917 | } 918 | idx += hdr.N * 3 + HDR99_SIZE + 2; 919 | } 920 | 921 | else if (type == 0xCF) 922 | { 923 | if (idx + hdr.N * 3 + HDR2_SIZE + 2 > len) 924 | { 925 | // need more bytes 926 | break; 927 | } 928 | RawDataHdr2 hdr2; 929 | memcpy(&hdr2, buf + idx, HDR2_SIZE); 930 | 931 | RawData *fan = GetData0xCF(hdr2, buf + idx, parser->with_chk); 932 | if (fan) 933 | { 934 | fans[*nfan] = fan; 935 | *nfan += 1; 936 | } 937 | idx += hdr.N * 3 + HDR2_SIZE + 2; 938 | } 939 | else if (type == 0xC7) 940 | { 941 | if (idx + hdr.N * 5 + HDR7_SIZE + 2 > len) 942 | { 943 | // need more bytes 944 | break; 945 | } 946 | RawDataHdr7 hdr7; 947 | memcpy(&hdr7, buf + idx, HDR7_SIZE); 948 | 949 | RawData *fan = GetData0xC7(parser, hdr7, buf + idx); 950 | if (fan) 951 | { 952 | fans[*nfan] = fan; 953 | *nfan += 1; 954 | } 955 | idx += hdr.N * 5 + HDR7_SIZE + 2; 956 | } 957 | else if (type == 0xAA) 958 | { 959 | if (idx + hdr.N * 5 + HDRAA_SIZE + 2 > len) 960 | { 961 | break; 962 | } 963 | RawDataHdrAA hdrAA; 964 | memcpy(&hdrAA, buf + idx, HDRAA_SIZE); 965 | 966 | RawData *fan = GetData0xAA(parser, hdrAA, buf + idx); 967 | if (fan) 968 | { 969 | fans[*nfan] = fan; 970 | *nfan += 1; 971 | } 972 | idx += hdr.N * 5 + HDRAA_SIZE + 2; 973 | } 974 | else if (type == 0xDF) 975 | { 976 | if (idx + hdr.N * 3 + HDR3_SIZE + 2 > len) 977 | { 978 | // need more bytes 979 | break; 980 | } 981 | RawDataHdr3 hdr3; 982 | memcpy(&hdr3, buf + idx, HDR3_SIZE); 983 | 984 | RawData *fan = GetData0xDF(hdr3, buf + idx, parser->with_chk); 985 | if (fan) 986 | { 987 | // printf("set [%d] %d\n", *nfan, fan->angle); 988 | fans[*nfan] = fan; 989 | *nfan += 1; 990 | } 991 | idx += hdr.N * 3 + HDR3_SIZE + 2; 992 | } 993 | else 994 | { 995 | // should not 996 | } 997 | } 998 | return idx - unk; 999 | } 1000 | 1001 | Parser *ParserOpen(int raw_bytes, bool with_chksum, int dev_id, 1002 | int error_circle, double error_scale, bool from_zero, int time_mode, 1003 | CommandList cmd, char *ip, int port) 1004 | { 1005 | Parser *parser = new Parser; 1006 | 1007 | parser->rest_len = 0; 1008 | parser->raw_mode = raw_bytes; 1009 | parser->with_chk = with_chksum; 1010 | parser->fan_segs_c7 = NULL; 1011 | parser->fan_segs_aa = NULL; 1012 | parser->dev_id = dev_id; 1013 | parser->error_circle = error_circle; 1014 | parser->error_scale = error_scale; 1015 | parser->cmd = cmd; 1016 | parser->is_from_zero = from_zero; 1017 | strcpy(parser->ip, ip); 1018 | parser->port = port; 1019 | g_timemode = time_mode; 1020 | parser->isrun = false; 1021 | return parser; 1022 | } 1023 | 1024 | // int ParserClose(HParser hP) 1025 | // { 1026 | // Parser *parser = (Parser *)hP; 1027 | // delete parser; 1028 | // return 0; 1029 | // } 1030 | 1031 | int ParserRunStream(Parser *hP, int len, unsigned char *bytes, RawData *fans[]) 1032 | { 1033 | Parser *parser = (Parser *)hP; 1034 | 1035 | int nfan = MAX_FANS; 1036 | 1037 | unsigned char *buf = new unsigned char[len + parser->rest_len]; 1038 | memset(buf, 0, sizeof(len + parser->rest_len)); 1039 | if (!buf) 1040 | { 1041 | // printf("out of memory\n"); 1042 | ERROR((int)MEMORY_SPACE_ERR, Error::GetErrorString(MEMORY_SPACE_ERR).c_str()); 1043 | return -1; 1044 | } 1045 | 1046 | if (parser->rest_len > 0) 1047 | { 1048 | memcpy(buf, parser->rest_buf, parser->rest_len); 1049 | } 1050 | memcpy(buf + parser->rest_len, bytes, len); 1051 | len += parser->rest_len; 1052 | 1053 | int used = ParseStream(parser, len, buf, &nfan, fans); 1054 | 1055 | #if 0 1056 | for (int i=0; iros_angle = LidarAng2ROS(fans[i]->angle + fans[i]->span); 1059 | } 1060 | #endif 1061 | 1062 | parser->rest_len = len - used; 1063 | if (parser->rest_len > 0) 1064 | { 1065 | memcpy(parser->rest_buf, buf + used, parser->rest_len); 1066 | } 1067 | 1068 | delete[] buf; 1069 | 1070 | return nfan; 1071 | } 1072 | int alarmProc(unsigned char *buf, int len) 1073 | { 1074 | if (memcmp(buf, "LMSG", 4) == 0) 1075 | { 1076 | if (len >= (int)sizeof(LidarAlarm)) 1077 | { 1078 | LidarAlarm *msg = (LidarAlarm *)buf; 1079 | if (msg->hdr.type >= 0x100) 1080 | { 1081 | if (getbit(msg->hdr.data, 12) == 1) 1082 | { 1083 | DEBUG("ALARM LEVEL:OBSERVE MSG TYPE:%d ZONE ACTIVE:%x", msg->hdr.type, msg->zone_actived); 1084 | } 1085 | if (getbit(msg->hdr.data, 13) == 1) 1086 | { 1087 | DEBUG("ALARM LEVEL:WARM MSG TYPE:%d ZONE ACTIVE:%x", msg->hdr.type, msg->zone_actived); 1088 | } 1089 | if (getbit(msg->hdr.data, 14) == 1) 1090 | { 1091 | DEBUG("ALARM LEVEL:ALARM MSG TYPE:%d ZONE ACTIVE:%x", msg->hdr.type, msg->zone_actived); 1092 | } 1093 | if (getbit(msg->hdr.data, 15) == 1) 1094 | { 1095 | DEBUG("ALARM COVER MSG TYPE:%d", msg->hdr.type); 1096 | } 1097 | if (getbit(msg->hdr.data, 16) == 1) 1098 | { 1099 | DEBUG("ALARM NO DATA MSG TYPE:%d", msg->hdr.type); 1100 | } 1101 | if (getbit(msg->hdr.data, 17) == 1) 1102 | { 1103 | // printf("ALARM ZONE NO ACTIVE MSG TYPE:%d\n", msg->hdr.type); 1104 | } 1105 | if (getbit(msg->hdr.data, 18) == 1) 1106 | { 1107 | DEBUG("ALARM SYSTEM ERROR MSG TYPE:%d", msg->hdr.type); 1108 | } 1109 | if (getbit(msg->hdr.data, 19) == 1) 1110 | { 1111 | DEBUG("ALARM RUN EXCEPTION MSG TYPE:%d", msg->hdr.type); 1112 | } 1113 | if (getbit(msg->hdr.data, 20) == 1) 1114 | { 1115 | DEBUG("ALARM NETWORK ERROR MSG TYPE:%d", msg->hdr.type); 1116 | } 1117 | if (getbit(msg->hdr.data, 21) == 1) 1118 | { 1119 | DEBUG("ALARM UPDATING MSG TYPE:%d", msg->hdr.type); 1120 | } 1121 | if (getbit(msg->hdr.data, 22) == 1) 1122 | { 1123 | DEBUG("ALARM ZERO POS ERROR MSG TYPE:%d", msg->hdr.type); 1124 | } 1125 | } 1126 | // 说明有LMSG_ERROR报错信息 1127 | if (msg->hdr.type % 2 == 1) 1128 | { 1129 | 1130 | if (getbit(msg->hdr.data, 0) == 1) 1131 | { 1132 | DEBUG("LIDAR LOW POWER MSG TYPE:%d", msg->hdr.type); 1133 | } 1134 | if (getbit(msg->hdr.data, 1) == 1) 1135 | { 1136 | DEBUG("LIDAR MOTOR STALL MSG TYPE:%d", msg->hdr.type); 1137 | } 1138 | if (getbit(msg->hdr.data, 2) == 1) 1139 | { 1140 | DEBUG("LIDAR RANGING MODULE TEMPERATURE HIGH MSG TYPE:%d", msg->hdr.type); 1141 | } 1142 | if (getbit(msg->hdr.data, 3) == 1) 1143 | { 1144 | DEBUG("LIDAR NETWORK ERROR MSG TYPE:%d", msg->hdr.type); 1145 | } 1146 | if (getbit(msg->hdr.data, 4) == 1) 1147 | { 1148 | DEBUG("LIDAR RANGER MODULE NO OUTPUT MSG TYPE:%d", msg->hdr.type); 1149 | } 1150 | } 1151 | } 1152 | 1153 | return 1; 1154 | } 1155 | 1156 | if (memcmp(buf, "AM", 2) == 0) 1157 | { 1158 | if (len >= (int)sizeof(PROCOTOL_HOST_ALARM_ST)) 1159 | { 1160 | PROCOTOL_HOST_ALARM_ST *msg = (PROCOTOL_HOST_ALARM_ST *)buf; 1161 | // crc校验有可能是脏数据 1162 | uint32_t crc = msg->crc; 1163 | uint32_t crc2 = stm32crc((unsigned int *)(msg), (sizeof(PROCOTOL_HOST_ALARM_ST) - 4) / 4); 1164 | if (crc == crc2) 1165 | { 1166 | // 错误信息 1167 | if (getbit(msg->events, 0) == 1) 1168 | { 1169 | // 底板低压 1170 | DEBUG("mainboard low voltage"); 1171 | } 1172 | if (getbit(msg->events, 1) == 1) 1173 | { 1174 | // 底板高压; 1175 | DEBUG("mainboard's voltage too high"); 1176 | } 1177 | if (getbit(msg->events, 2) == 1) 1178 | { 1179 | // 机头高温; 1180 | DEBUG("The distance measuring module is too hot"); 1181 | } 1182 | if (getbit(msg->events, 3) == 1) 1183 | { 1184 | // 机头低压; 1185 | DEBUG("Ranging Module low voltage"); 1186 | } 1187 | if (getbit(msg->events, 4) == 1) 1188 | { 1189 | // 机头高压; 1190 | DEBUG("Ranging Module's voltage too high"); 1191 | } 1192 | if (getbit(msg->events, 5) == 1) 1193 | { 1194 | // 机头无数据; 1195 | DEBUG("Ranging Module is Muted"); 1196 | } 1197 | if (getbit(msg->events, 6) == 1) 1198 | { 1199 | // 机头数据异常; 1200 | DEBUG("Ranging Module Communication error"); 1201 | } 1202 | if (getbit(msg->events, 7) == 1) 1203 | { 1204 | // 红外接收异常; 1205 | DEBUG("IR Communication error"); 1206 | } 1207 | if (getbit(msg->events, 8) == 1) 1208 | { 1209 | // 线圈断开; 1210 | DEBUG("Wireless Power Supply Lost"); 1211 | } 1212 | if (getbit(msg->events, 9) == 1) 1213 | { 1214 | // 线圈过流; 1215 | DEBUG("Wireless Power Supply overcurrent"); 1216 | } 1217 | if (getbit(msg->events, 10) == 1) 1218 | { 1219 | // 码盘错误; 1220 | DEBUG("encoder exception"); 1221 | } 1222 | if (getbit(msg->events, 11) == 1) 1223 | { 1224 | // 电机异常; 1225 | DEBUG("motor exception"); 1226 | } 1227 | if (getbit(msg->events, 16) == 1) 1228 | { 1229 | // 观察区; 1230 | DEBUG("Observe"); 1231 | } 1232 | if (getbit(msg->events, 17) == 1) 1233 | { 1234 | // 警戒区; 1235 | DEBUG("Warning"); 1236 | } 1237 | if (getbit(msg->events, 18) == 1) 1238 | { 1239 | // 报警区; 1240 | DEBUG("Alarm"); 1241 | } 1242 | if (getbit(msg->events, 19) == 1) 1243 | { 1244 | // 无防区设置; 1245 | DEBUG("NO Zone Set"); 1246 | } 1247 | if (getbit(msg->events, 20) == 1) 1248 | { 1249 | // 雷达遮挡; 1250 | DEBUG("Lidar cover"); 1251 | } 1252 | 1253 | return 1; 1254 | } 1255 | } 1256 | } 1257 | 1258 | return 0; 1259 | } 1260 | int ParserRun(LidarNode hP, int len, unsigned char *buf, RawData *fans[]) 1261 | { 1262 | Parser *parser = (Parser *)hP.hParser; 1263 | 1264 | uint8_t type = buf[0]; 1265 | if (alarmProc(buf, len)) 1266 | return 0; 1267 | 1268 | if (buf[1] != 0xfa) 1269 | { 1270 | // printf("skip packet %x %x len\n", buf[0], buf[1]); 1271 | } 1272 | else if (buf[0] == 0x88) 1273 | { 1274 | PacketUart hdr; 1275 | memcpy(&hdr, buf, sizeof(PacketUart)); 1276 | if ((unsigned int)len >= hdr.len + sizeof(PacketUart)) 1277 | { 1278 | if (parser->dev_id != (u_int32_t)ANYONE && hdr.dev_id != parser->dev_id) 1279 | { 1280 | // not my data 1281 | return 0; 1282 | } 1283 | return ParserRunStream(hP.hParser, hdr.len, buf + sizeof(PacketUart), fans); 1284 | } 1285 | } 1286 | else if (type == 0xCE) 1287 | { 1288 | RawDataHdr hdr; 1289 | memcpy(&hdr, buf, HDR_SIZE); 1290 | 1291 | if (HDR_SIZE + hdr.N * 2 + 2 == len) 1292 | { 1293 | RawData *fan = GetData0xCE_2(hdr, buf, parser->flags, parser->with_chk); 1294 | if (fan) 1295 | { 1296 | fans[0] = fan; 1297 | return 1; 1298 | } 1299 | } 1300 | else if (HDR_SIZE + hdr.N * 3 + 2 == len) 1301 | { 1302 | RawData *fan = GetData0xCE_3(hdr, buf, parser->flags, parser->with_chk); 1303 | if (fan) 1304 | { 1305 | fans[0] = fan; 1306 | return 1; 1307 | } 1308 | } 1309 | else 1310 | DEBUG("CE len %d N %d\n", len, hdr.N); 1311 | } 1312 | else if (type == 0xCF) 1313 | { 1314 | RawDataHdr2 hdr; 1315 | memcpy(&hdr, buf, HDR2_SIZE); 1316 | 1317 | if (hdr.N * 3 + HDR2_SIZE + 2 > len) 1318 | { 1319 | // need more bytes 1320 | DEBUG("CF len %d N %d\n", len, hdr.N); 1321 | return 0; 1322 | } 1323 | 1324 | RawData *fan = GetData0xCF(hdr, buf, parser->with_chk); 1325 | if (fan) 1326 | { 1327 | // printf("CF %d + %d %d\n", fan->angle, fan->span, fan->N); 1328 | fans[0] = fan; 1329 | return 1; 1330 | } 1331 | } 1332 | else if (type == 0xDF) 1333 | { 1334 | RawDataHdr3 hdr; 1335 | memcpy(&hdr, buf, HDR3_SIZE); 1336 | 1337 | if (hdr.N * 3 + HDR3_SIZE + 2 > len) 1338 | { 1339 | // need more bytes 1340 | DEBUG("DF len %d N %d\n", len, hdr.N); 1341 | return 0; 1342 | } 1343 | 1344 | RawData *fan = GetData0xDF(hdr, buf, parser->with_chk); 1345 | if (fan) 1346 | { 1347 | // printf("set [%d] %d\n", *nfan, fan->angle); 1348 | fans[0] = fan; 1349 | return 1; 1350 | } 1351 | } 1352 | else if (type == 0xC7) 1353 | { 1354 | RawDataHdr7 hdr; 1355 | memcpy(&hdr, buf, HDR7_SIZE); 1356 | 1357 | if (hdr.N * 5 + HDR7_SIZE + 2 > len) 1358 | { 1359 | // need more bytes 1360 | // printf("C7 len %d N %d\n", len, hdr.N); 1361 | return 0; 1362 | } 1363 | 1364 | RawData *fan = GetData0xC7(parser, hdr, buf); 1365 | if (fan) 1366 | { 1367 | // printf("set [%d] %d\n", *nfan, fan->angle); 1368 | fans[0] = fan; 1369 | return 1; 1370 | } 1371 | return 0; 1372 | } 1373 | else if (type == 0xAA) 1374 | { 1375 | 1376 | RawDataHdrAA hdr; 1377 | memcpy(&hdr, buf, HDRAA_SIZE); 1378 | 1379 | if (hdr.N * 5 + HDRAA_SIZE + 2 > len) 1380 | { 1381 | // need more bytes 1382 | // printf("C7 len %d N %d\n", len, hdr.N); 1383 | return 0; 1384 | } 1385 | 1386 | RawData *fan = GetData0xAA(parser, hdr, buf); 1387 | if (fan) 1388 | { 1389 | // printf("set [%d] %d\n",fan->N, fan->angle); 1390 | fans[0] = fan; 1391 | return 1; 1392 | } 1393 | return 0; 1394 | } 1395 | else if (type == 0x99) 1396 | { 1397 | RawDataHdr99 hdr; 1398 | memcpy(&hdr, buf, HDR99_SIZE); 1399 | 1400 | if (hdr.N * 3 + HDR99_SIZE + 2 > len) 1401 | { 1402 | // need more bytes 1403 | // printf("99 len %d N %d\n", len, hdr.N); 1404 | return 0; 1405 | } 1406 | 1407 | RawData *fan = GetData0x99(hdr, buf, parser->with_chk); 1408 | if (fan) 1409 | { 1410 | // printf("set [%d] %d\n", *nfan, fan->angle); 1411 | fans[0] = fan; 1412 | return 1; 1413 | } 1414 | return 0; 1415 | } 1416 | 1417 | if (buf[0] == 0x4c && buf[1] == 0x48) 1418 | { 1419 | // 1420 | return 0; 1421 | } 1422 | if (buf[0] == 0x4f && buf[1] == 0x4f && buf[2] == 0x42 && buf[3] == 0x53) 1423 | { 1424 | return 0; 1425 | } 1426 | DEBUG("skip packet %08x len %d\n", *(uint32_t *)buf, len); 1427 | // printf("qwer:%02X %02X %02X %02X\n", buf[0],buf[1],buf[2],buf[3]); 1428 | return 0; 1429 | } 1430 | 1431 | int strip(const char *s, char *buf) 1432 | { 1433 | int len = 0; 1434 | for (int i = 0; s[i] != 0; i++) 1435 | { 1436 | if (s[i] >= 'a' && s[i] <= 'z') 1437 | buf[len++] = s[i]; 1438 | else if (s[i] >= 'A' && s[i] <= 'Z') 1439 | buf[len++] = s[i]; 1440 | else if (s[i] >= '0' && s[i] <= '9') 1441 | buf[len++] = s[i]; 1442 | else if (s[i] == '-') 1443 | buf[len++] = s[i]; 1444 | else if (len > 0) 1445 | break; 1446 | } 1447 | buf[len] = 0; 1448 | return len; 1449 | } 1450 | 1451 | int mkpathAll(std::string s, mode_t mode = 0755) 1452 | { 1453 | size_t pre = 0, pos; 1454 | std::string dir; 1455 | int mdret; 1456 | 1457 | if (s[s.size() - 1] != '/') 1458 | { 1459 | // forctrailing / so we can handle everything in loop 1460 | s += '/'; 1461 | } 1462 | 1463 | while ((pos = s.find_first_of('/', pre)) != std::string::npos) 1464 | { 1465 | dir = s.substr(0, pos++); 1466 | pre = pos; 1467 | if (dir.size() == 0) 1468 | continue; // if leading / first time is 0 length 1469 | if ((mdret = ::mkdir(dir.c_str(), mode)) && errno != EEXIST) 1470 | { 1471 | return mdret; 1472 | } 1473 | } 1474 | return mdret; 1475 | } 1476 | std::string stringfilter(char *str, int num) 1477 | { 1478 | int index = 0; 1479 | for (int i = 0; i < num; i++) 1480 | { 1481 | if ((str[i] >= 45 && str[i] <= 58) || (str[i] >= 65 && str[i] <= 90) || (str[i] >= 97 && str[i] <= 122) || str[i] == 32 || str[i] == '_') 1482 | { 1483 | index++; 1484 | } 1485 | else 1486 | { 1487 | std::string arr = str; 1488 | arr = arr.substr(0, index); 1489 | return arr; 1490 | } 1491 | } 1492 | return ""; 1493 | } 1494 | 1495 | int find(std::vector a, int n, int x) 1496 | { 1497 | int i; 1498 | int min = abs(a.at(0).angle - x); 1499 | int r = 0; 1500 | 1501 | for (i = 0; i < n; ++i) 1502 | { 1503 | if (abs(a.at(i).angle - x) < min) 1504 | { 1505 | min = abs(a.at(i).angle - x); 1506 | r = i; 1507 | } 1508 | } 1509 | 1510 | return a[r].angle; 1511 | } 1512 | // int autoGetFirstAngle(RawData raw, bool from_zero, std::vector &raws, std::string &result) 1513 | // { 1514 | // int angles = 0; 1515 | // int size = raws.size(); 1516 | // // printf("angle %d size:%d\n", raw.angle,size); 1517 | // if (size >= 1) 1518 | // { 1519 | // RawData tmp = raws.at(size - 1); 1520 | // RawData tmp2 = raws.at(0); 1521 | // if (raw.angle == tmp2.angle) 1522 | // { 1523 | // for (int i = 0; i < size; i++) 1524 | // { 1525 | // tmp = raws.at(i); 1526 | // angles += tmp.span; 1527 | // } 1528 | // if (angles != 3600) 1529 | // { 1530 | // // result="angle sum "+std::to_string(angles); 1531 | // // printf("angle sum %d size:%d\n", angles,size); 1532 | // raws.clear(); 1533 | // return -2; 1534 | // } 1535 | // else 1536 | // { 1537 | // int ret = -1; 1538 | // if (from_zero) 1539 | // ret = find(raws, raws.size(), 0); 1540 | // else 1541 | // ret = find(raws, raws.size(), 1800); 1542 | 1543 | // raws.clear(); 1544 | // return ret; 1545 | // } 1546 | // } 1547 | // if (raw.angle == (tmp.angle + tmp.span) % 3600) 1548 | // { 1549 | // // 说明是连续的扇区 1550 | // raws.push_back(raw); 1551 | // } 1552 | // } 1553 | // else 1554 | // raws.push_back(raw); 1555 | // return -1; 1556 | // } 1557 | // 获取起始点位坐标的位置 1558 | int getFirstidx(RawData raw, int angle) 1559 | { 1560 | int idx = -1; 1561 | if ((raw.angle <= angle * 10) && ((raw.angle + raw.span) > angle * 10)) 1562 | { 1563 | // 下标减一 1564 | idx = 1.0 * (angle * 10 - raw.angle) / raw.span * raw.N; 1565 | } 1566 | return idx; 1567 | } 1568 | 1569 | void PublishData(PubHub *pub, int n, RawData **fans) 1570 | { 1571 | int skip = 0; 1572 | RawData *drop[MAX_FANS]; 1573 | PubHub *hub = (PubHub *)pub; 1574 | 1575 | pthread_mutex_lock(&hub->mtx); 1576 | 1577 | // gettimeofday(&begin,NULL); 1578 | // long long beginTime = (long long)begin.tv_sec * 1000000 + (long long)begin.tv_usec; 1579 | 1580 | if (hub->nfan + n > MAX_FANS) 1581 | { 1582 | int nr = hub->nfan + n - MAX_FANS; 1583 | 1584 | for (int i = 0; i < nr; i++) 1585 | drop[skip++] = hub->fans[i]; 1586 | 1587 | for (int i = nr; i < hub->nfan; i++) 1588 | { 1589 | hub->fans[i - nr] = hub->fans[i]; 1590 | } 1591 | 1592 | hub->nfan -= nr; 1593 | } 1594 | 1595 | for (int i = 0; i < n; i++) 1596 | { 1597 | hub->fans[hub->nfan++] = fans[i]; 1598 | } 1599 | 1600 | // gettimeofday(&begin,NULL); 1601 | // long long end = (long long)begin.tv_sec * 1000000 + (long long)begin.tv_usec; 1602 | // DEBUG("time 2 is:%ld\n",end-beginTime); 1603 | 1604 | pthread_mutex_unlock(&hub->mtx); 1605 | 1606 | for (int i = 0; i < skip; i++) 1607 | { 1608 | delete drop[i]; 1609 | } 1610 | } 1611 | void SetTimeStamp(RawData *dat) 1612 | { 1613 | timeval t; 1614 | gettimeofday(&t, NULL); 1615 | dat->ts[0] = t.tv_sec; 1616 | dat->ts[1] = t.tv_usec * 1000; 1617 | } 1618 | 1619 | void redirect_stdout_to_log(const char *path) 1620 | { 1621 | int fd = open(path, O_WRONLY | O_CREAT | O_TRUNC, 0644); 1622 | if (fd == -1) { 1623 | perror("open log file failed"); 1624 | exit(EXIT_FAILURE); 1625 | } 1626 | dup2(fd, STDOUT_FILENO); // 重定向 stdout 1627 | dup2(fd, STDERR_FILENO); // 可选：重定向 stderr 1628 | close(fd); 1629 | } --------------------------------------------------------------------------------