diff --git a/Lslidar_ROS2_driver/README.md b/Lslidar_ROS2_driver/README.md new file mode 100644 index 0000000..2dbf42e --- /dev/null +++ b/Lslidar_ROS2_driver/README.md @@ -0,0 +1,44 @@ +# lslidar + +## Description +The `lslidar package is a linux ROS2 driver for lslidar M10 ,M10_GPS,M10_P,M10_PLUS and N10. +The package is tested on Ubuntu 20.04 with ROS2 FOXY. + +## Compling +This is a Catkin package. Make sure the package is on `ROS_PACKAGE_PATH` after cloning the package to your workspace. And the normal procedure for compling a catkin package will work. + +``` +cd your_work_space +colcon build +source install/setup.bash +ros2 launch lslidar_driver lslidar_launch.py +``` +open new terminal +ros2 topic pub -1 /lslidar_order std_msgs/msg/Int8 data:\ 1\ (open radar) +ros2 topic pub -1 /lslidar_order std_msgs/msg/Int8 data:\ 0\ (close radar) + + + + + +ros2 launch lslidar_driver lslidar_launch.py + +``` + +Note that this launch file launches both the driver, which is the only launch file needed to be used. + + +## FAQ + + +## Bug Report + +Prefer to open an issue. You can also send an E-mail to honghangli@lslidar.com + + + + +RERTION + + + diff --git a/Lslidar_ROS2_driver/lslidar_driver/CMakeLists.txt b/Lslidar_ROS2_driver/lslidar_driver/CMakeLists.txt new file mode 100644 index 0000000..85e13f6 --- /dev/null +++ b/Lslidar_ROS2_driver/lslidar_driver/CMakeLists.txt @@ -0,0 +1,56 @@ +cmake_minimum_required(VERSION 3.5) +project(lslidar_driver) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +set(libpcap_LIBRARIES -lpcap) + +#set(FastRTPS_INCLUDE_DIR /opt/ros/foxy/include) +#set(FastRTPS_LIBRARY_RELEASE /opt/ros/foxy/lib/libfastrtps.so) + +#find_package(Boost REQUIRED COMPONENTS ) +find_package(Boost REQUIRED thread) +find_package(rclcpp REQUIRED) +find_package(PCL REQUIRED) +find_package(diagnostic_updater REQUIRED) +find_package(lslidar_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(ament_cmake REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclpy REQUIRED) +find_package(pcl_conversions REQUIRED) +find_package(sensor_msgs REQUIRED) +#find_package(PCL REQUIRED COMPONENTS common io) + +include_directories( + include + ${PCL_INCLUDE_DIRS} + ${PCL_COMMON_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} +) + +# Node +add_executable(lslidar_driver_node src/lslidar_driver_node.cc src/lslidar_driver.cc src/input.cc src/lsiosr.cpp) +target_link_libraries(lslidar_driver_node ${rclcpp_LIBRARIES} ${libpcap_LIBRARIES} ${Boost_LIBRARIES} Boost::thread) +ament_target_dependencies(lslidar_driver_node rclcpp std_msgs lslidar_msgs sensor_msgs diagnostic_updater pcl_conversions) + + +install(DIRECTORY launch params rviz + DESTINATION share/${PROJECT_NAME}) + +install(TARGETS + lslidar_driver_node + DESTINATION lib/${PROJECT_NAME} +) + +ament_export_dependencies(rclcpp pluginlib lslidar_msgs sensor_msgs pcl_conversions) +ament_export_include_directories(include ${PCL_COMMON_INCLUDE_DIRS}) + +ament_package() diff --git a/Lslidar_ROS2_driver/lslidar_driver/include/lslidar_driver/input.h b/Lslidar_ROS2_driver/lslidar_driver/include/lslidar_driver/input.h new file mode 100644 index 0000000..2715da4 --- /dev/null +++ b/Lslidar_ROS2_driver/lslidar_driver/include/lslidar_driver/input.h @@ -0,0 +1,134 @@ +/* + * This file is part of lslidar_ch driver. + * + * The driver is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * The driver is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with the driver. If not, see . + * + * Input -- base class used to access the data independently of + * its source + * + * InputSocket -- derived class reads live data from the device + * via a UDP socket + * + * InputPCAP -- derived class provides a similar interface from a + * PCAP dump + */ + +#ifndef __LSLIDAR_INPUT_H_ +#define __LSLIDAR_INPUT_H_ + +#include +#include +#include +#include +#include "rclcpp/rclcpp.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace lslidar_driver +{ +static uint16_t MSOP_DATA_PORT_NUMBER = 2368; // lslidar default data port on PC +/** + * 从在线的网络数据或离线的网络抓包数据（pcap文件）中提取出lidar的原始数据，即packet数据包 + * @brief The Input class, + * + * @param private_nh 一个NodeHandled,用于通过节点传递参数 + * @param port + * @returns 0 if successful, + * -1 if end of file + * >0 if incomplete packet (is this possible?) + */ +class Input +{ +public: + Input(rclcpp::Node* private_nh, uint16_t port); + + virtual ~Input() + { + } + + virtual int getPacket(lslidar_msgs::msg::LslidarPacket::UniquePtr &packet) = 0; + + int getRpm(void); + int getReturnMode(void); + bool getUpdateFlag(void); + void clearUpdateFlag(void); + void UDP_order(const std_msgs::msg::Int8 msg); + void UDP_difop(); +protected: + rclcpp::Node* private_nh_; + uint16_t port_; + std::string devip_str_; + std::string lidar_name; + int cur_rpm_; + int return_mode_; + bool npkt_update_flag_; + bool add_multicast; + std::string group_ip; + int UDP_PORT_NUMBER_DIFOP; + int socket_id_difop; + int sockfd_; + std::string devip_str_difop; +}; + +/** @brief Live lslidar input from socket. */ +class InputSocket : public Input +{ +public: + InputSocket(rclcpp::Node* private_nh, uint16_t port = MSOP_DATA_PORT_NUMBER); + + virtual ~InputSocket(); + + virtual int getPacket(lslidar_msgs::msg::LslidarPacket::UniquePtr &packet); + +private: +private: + + in_addr devip_; + in_addr devip_difop; + //struct ip_mreq group; + +}; +class InputPCAP : public Input +{ +public: + InputPCAP(rclcpp::Node* private_nh,uint16_t port = MSOP_DATA_PORT_NUMBER, double packet_rate = 0.0, + std::string filename=""); + virtual ~InputPCAP(); + virtual int getPacket(lslidar_msgs::msg::LslidarPacket::UniquePtr &pkt); +private: + + rclcpp::Rate packet_rate_; + std::string filename_; + pcap_t *pcap_; + bpf_program pcap_packet_filter_; + char errbuf_[PCAP_ERRBUF_SIZE]; + bool empty_; + bool read_once_; + bool read_fast_; + double repeat_delay_; + }; +} + +#endif // __LSLIDAR_INPUT_H diff --git a/Lslidar_ROS2_driver/lslidar_driver/include/lslidar_driver/lsiosr.h b/Lslidar_ROS2_driver/lslidar_driver/include/lslidar_driver/lsiosr.h new file mode 100644 index 0000000..6109497 --- /dev/null +++ b/Lslidar_ROS2_driver/lslidar_driver/include/lslidar_driver/lsiosr.h @@ -0,0 +1,88 @@ +/******************************************************* +@company: Copyright (C) 2021, Leishen Intelligent System +@product: LSM10_N10 +@filename: lsiosr.cpp +@brief: +@version: date: author: comments: +@v1.0 22-10-24 li new +*******************************************************/ +#ifndef LSIOSR_H +#define LSIOSR_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +//波特率 +#define BAUD_230400 230400 +#define BAUD_460800 460800 +#define BAUD_500000 500000 +#define BAUD_921600 921600 + +//奇偶校验位 +#define PARITY_ODD 'O' //奇数 +#define PARITY_EVEN 'E' //偶数 +#define PARITY_NONE 'N' //无奇偶校验位 + +//停止位 +#define STOP_BIT_1 1 +#define STOP_BIT_2 2 + +//数据位 +#define DATA_BIT_7 7 +#define DATA_BIT_8 8 + +namespace lslidar_driver +{ +class LSIOSR{ +public: + static LSIOSR* instance(std::string name, int speed, int fd = 0); + + ~LSIOSR(); + + /* 从串口中读取数据 */ + int read(unsigned char *buffer, int length, int timeout = 30); + + /* 向串口传数据 */ + int send(const char* buffer, int length, int timeout = 30); + + /* Empty serial port input buffer */ + void flushinput(); + + /* 串口初始化 */ + int init(); + + int close(); + + /* 获取串口号 */ + std::string getPort(); + + /* 设置串口号 */ + int setPortName(std::string name); + +private: + LSIOSR(std::string name, int speed, int fd); + + int waitWritable(int millis); + int waitReadable(int millis); + + /* 串口配置的函数 */ + int setOpt(int nBits, uint8_t nEvent, int nStop); + + std::string port_; + int baud_rate_; + + int fd_; +}; +} +#endif + diff --git a/Lslidar_ROS2_driver/lslidar_driver/include/lslidar_driver/lslidar_driver.h b/Lslidar_ROS2_driver/lslidar_driver/include/lslidar_driver/lslidar_driver.h new file mode 100644 index 0000000..1ec59d0 --- /dev/null +++ b/Lslidar_ROS2_driver/lslidar_driver/include/lslidar_driver/lslidar_driver.h @@ -0,0 +1,159 @@ +/* + * This file is part of lslidar driver. + * + * The driver is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * The driver is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with the driver. If not, see . + */ + +#ifndef LSLIDAR_DRIVER_H +#define LSLIDAR_DRIVER_H + +#include +#include +#include +#include + +#include +#include +#include +#include "rclcpp/rclcpp.hpp" +#include +#include "diagnostic_updater/diagnostic_updater.hpp" +#include "diagnostic_updater/publisher.hpp" +#include "lslidar_msgs/msg/lslidar_packet.hpp" +#include "std_msgs/msg/byte.hpp" + +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "pcl_conversions/pcl_conversions.h" +#include "pcl/point_types.h" + +#include "time.h" +#include "input.h" +#include "lsiosr.h" +#include "sensor_msgs/msg/laser_scan.hpp" +namespace lslidar_driver { + +struct PointXYZIT { + PCL_ADD_POINT4D; + uint8_t intensity; + double timestamp; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW // make sure our new allocators are aligned +} EIGEN_ALIGN16; + +typedef struct { + double degree; + double range; + double intensity; +} ScanPoint; + +class LslidarDriver: public rclcpp::Node { +public: + LslidarDriver(); + LslidarDriver(const rclcpp::NodeOptions& options); + ~LslidarDriver(); + + bool initialize(); + bool polling(); + + typedef std::shared_ptr LslidarDriverPtr; + typedef std::shared_ptr LslidarDriverConstPtr; + +private: + uint64_t get_gps_stamp(struct tm t); + uint8_t N10_CalCRC8(unsigned char * p, int len); + bool loadParameters(); + bool createRosIO(); + void open_serial(); + void lidar_difop(); + void lidar_order(const std_msgs::msg::Int8::SharedPtr msg); + void data_processing(unsigned char *packet_bytes,int len); + void data_processing_2(unsigned char *packet_bytes,int len); + void difop_processing(unsigned char *packet_bytes); + void pubScanThread(); + void recvThread_crc(int &count,int &link_time); + int receive_data(unsigned char *packet_bytes); + int getScan(std::vector &points, rclcpp::Time &scan_time, float &scan_duration); + + boost::thread *pubscan_thread_ ; + boost::shared_ptr msop_input_; + boost::mutex mutex_; + boost::mutex pubscan_mutex_; + boost::condition_variable pubscan_cond_; + + int UDP_PORT_NUMBER; + int count_num; + int package_points; + int data_bits_start; + int degree_bits_start; + int end_degree_bits_start; + int rpm_bits_start; + int baud_rate_; + int points_size_; + int idx = 0; + int link_time = 0; + + bool use_gps_ts; + bool is_start; + bool high_reflection; + bool compensation; + bool first_compensation = true; + bool pubScan; + bool pubPointCloud2; + + double min_range; + double max_range; + double angle_disable_min; + double angle_disable_max; + double angle_able_min; + double angle_able_max; + double last_degree = 0.0; + double degree_compensation = 0.0; + + uint16_t PACKET_SIZE ; + uint64_t sweep_end_time_gps; + uint64_t sweep_end_time_hardware; + uint64_t sub_second; + + std::string frame_id; + std::string interface_selection; + std::string scan_topic; + std::string lidar_name; + std::string serial_port_; + std::string dump_file; + std::string pointcloud_topic; + std::string in_file_name; + + tm pTime; + rclcpp::Time pre_time_; + rclcpp::Time time_; + std::vector scan_points_; + std::vector scan_points_bak_; + // Diagnostics updater + diagnostic_updater::Updater diagnostics; + std::shared_ptr diag_topic; + double diag_min_freq; + double diag_max_freq; + rclcpp::Publisher::SharedPtr scan_pub; + rclcpp::Publisher::SharedPtr point_cloud_pub; + rclcpp::Subscription::SharedPtr difop_switch; + LSIOSR * serial_; +}; +typedef PointXYZIT VPoint; +typedef pcl::PointCloud VPointCloud; + +} // namespace lslidar_driver +POINT_CLOUD_REGISTER_POINT_STRUCT(lslidar_driver::PointXYZIT, + (float, x, x)(float, y, y)(float, z, z)( + std::uint8_t, intensity, + intensity)(double, timestamp, timestamp)) +#endif // _LSLIDAR_DRIVER_H_ diff --git a/Lslidar_ROS2_driver/lslidar_driver/launch/lslidar_double_launch.py b/Lslidar_ROS2_driver/lslidar_driver/launch/lslidar_double_launch.py new file mode 100644 index 0000000..f1361fb --- /dev/null +++ b/Lslidar_ROS2_driver/lslidar_driver/launch/lslidar_double_launch.py @@ -0,0 +1,50 @@ +#!/usr/bin/python3 +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import LifecycleNode +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument + +import lifecycle_msgs.msg +import os + +def generate_launch_description(): + + driver_dir_1 = os.path.join(get_package_share_directory('lslidar_driver'), 'params', 'lsx10_1.yaml') + driver_dir_2 = os.path.join(get_package_share_directory('lslidar_driver'), 'params', 'lsx10_2.yaml') + + driver_node_1 = LifecycleNode(package='lslidar_driver', + executable='lslidar_driver_node', + name='lslidar_driver_node', #设置激光数据topic名称 + output='screen', + emulate_tty=True, + namespace='lidar_1', + parameters=[driver_dir_1], + ) + + driver_node_2 = LifecycleNode(package='lslidar_driver', + executable='lslidar_driver_node', + name='lslidar_driver_node', #设置激光数据topic名称 + output='screen', + emulate_tty=True, + namespace='lidar_2', + parameters=[driver_dir_2], + ) + + rviz_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'rviz', 'lslidar.rviz') + + rviz_node = Node( + package='rviz2', + namespace='', + executable='rviz2', + name='rviz2', + arguments=['-d', rviz_dir], + output='screen') + + return LaunchDescription([ + driver_node_1, + driver_node_2, + rviz_node, + ]) + diff --git a/Lslidar_ROS2_driver/lslidar_driver/launch/lslidar_launch.py b/Lslidar_ROS2_driver/lslidar_driver/launch/lslidar_launch.py new file mode 100644 index 0000000..8933424 --- /dev/null +++ b/Lslidar_ROS2_driver/lslidar_driver/launch/lslidar_launch.py @@ -0,0 +1,40 @@ +#!/usr/bin/python3 +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import LifecycleNode +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument + +import lifecycle_msgs.msg +import os + +def generate_launch_description(): + + driver_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'params', 'lsx10.yaml') + + driver_node = LifecycleNode(package='lslidar_driver', + executable='lslidar_driver_node', + name='lslidar_driver_node', #设置激光数据topic名称 + output='screen', + emulate_tty=True, + namespace='', + parameters=[driver_dir], + ) + + + rviz_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'rviz', 'lslidar.rviz') + + rviz_node = Node( + package='rviz2', + namespace='', + executable='rviz2', + name='rviz2', + arguments=['-d', rviz_dir], + output='screen') + + return LaunchDescription([ + driver_node, + rviz_node, + ]) + diff --git a/Lslidar_ROS2_driver/lslidar_driver/package.xml b/Lslidar_ROS2_driver/lslidar_driver/package.xml new file mode 100644 index 0000000..f0abb26 --- /dev/null +++ b/Lslidar_ROS2_driver/lslidar_driver/package.xml @@ -0,0 +1,36 @@ + + + lslidar_driver + 1.2.0 + ROS device driver for Leishen lidar. + Nick Shu + Nick Shu + GNU General Public License V3.0 + + ament_cmake + + rclcpp + std_msgs + lslidar_msgs + pcl_conversions + rclpy + libpcap + libpcl-all-dev + pluginlib + sensor_msgs + + rclcpp + std_msgs + lslidar_msgs + pcl_conversions + rclpy + libpcap + libpcl-all + pluginlib + sensor_msgs + + diagnostic_updater + + ament_cmake + + \ No newline at end of file diff --git a/Lslidar_ROS2_driver/lslidar_driver/params/lsx10.yaml b/Lslidar_ROS2_driver/lslidar_driver/params/lsx10.yaml new file mode 100644 index 0000000..3fb00f9 --- /dev/null +++ b/Lslidar_ROS2_driver/lslidar_driver/params/lsx10.yaml @@ -0,0 +1,25 @@ +/lslidar_driver_node: + ros__parameters: + frame_id: laser_link #激光坐标 + group_ip: 224.1.1.2 + add_multicast: false + device_ip: 192.168.1.200 #雷达源IP + device_ip_difop: 192.168.1.102 #雷达目的ip + msop_port: 2368 #雷达目的端口号 + difop_port: 2369 #雷达源端口号 + lidar_name: M10 #雷达选择:M10 M10_P M10_PLUS M10_GPS N10 L10 N10_P + angle_disable_min: 0.0 #角度裁剪开始值 + angle_disable_max: 0.0 #角度裁剪结束值 + min_range: 0.0 #雷达接收距离最小值 + max_range: 200.0 #雷达接收距离最大值 + use_gps_ts: false #雷达是否使用GPS授时 + scan_topic: /scan #设置激光数据topic名称 + interface_selection: serial #接口选择:net 为网口,serial 为串口。 + serial_port_: /dev/ttyUSB0 #串口连接时的串口号 + high_reflection: false #M10_P雷达需填写该值,若不确定，请联系技术支持。 + compensation: false #M10系列是否使用角度补偿功能 + pubScan: true #是否发布scan话题 + pubPointCloud2: false #是否发布pointcloud2话题 + pointcloud_topic: /lslidar_point_cloud #设置激光数据topic名称 +# pcap: /home/ls/1.pcap #雷达是否使用pcap包读取功能 +# in_file_name: /home/ls/1.txt #雷达是否使用txt文件读取功能 \ No newline at end of file diff --git a/Lslidar_ROS2_driver/lslidar_driver/params/lsx10_1.yaml b/Lslidar_ROS2_driver/lslidar_driver/params/lsx10_1.yaml new file mode 100644 index 0000000..dbeeafa --- /dev/null +++ b/Lslidar_ROS2_driver/lslidar_driver/params/lsx10_1.yaml @@ -0,0 +1,25 @@ +/lidar_1/lslidar_driver_node: + ros__parameters: + frame_id: laser_link #激光坐标 + group_ip: 224.1.1.2 + add_multicast: false + device_ip: 192.168.1.200 #雷达源ip + device_ip_difop: 192.168.1.102 #雷达目的IP + msop_port: 2368 #雷达目的端口号 + difop_port: 2369 #雷达源端口号 + lidar_name: M10 #雷达选择:M10 M10_P M10_PLUS M10_GPS N10 L10 + angle_disable_min: 0.0 #角度裁剪开始值 + angle_disable_max: 90.0 #角度裁剪结束值 + min_range: 0.0 #雷达接收距离最小值 + max_range: 200.0 #雷达接收距离最大值 + use_gps_ts: false #雷达是否使用GPS授时 + scan_topic: /scan #设置激光数据topic名称 + interface_selection: serial #接口选择:net 为网口,serial 为串口。 + serial_port_: /dev/ttyUSB0 #串口连接时的串口号 + high_reflection: false #M10_P雷达需填写该值,若不确定，请联系技术支持。 + compensation: true #M10系列是否使用角度补偿功能 + pubScan: true #是否发布scan话题 + pubPointCloud2: true #是否发布pointcloud2话题 + pointcloud_topic: /lslidar_point_cloud #设置激光数据topic名称 +# pcap: /home/ls/work/2211/M10_P_gps.pcap #雷达是否使用pcap包读取功能 +# in_file_name: /home/ls/1.txt #雷达是否使用txt文件读取功能 \ No newline at end of file diff --git a/Lslidar_ROS2_driver/lslidar_driver/params/lsx10_2.yaml b/Lslidar_ROS2_driver/lslidar_driver/params/lsx10_2.yaml new file mode 100644 index 0000000..84a6e7b --- /dev/null +++ b/Lslidar_ROS2_driver/lslidar_driver/params/lsx10_2.yaml @@ -0,0 +1,25 @@ +/lidar_2/lslidar_driver_node: + ros__parameters: + frame_id: laser_link #激光坐标 + group_ip: 224.1.1.2 + add_multicast: false + device_ip: 192.168.1.200 #雷达源ip + device_ip_difop: 192.168.1.102 #雷达目的IP + msop_port: 2368 #雷达目的端口号 + difop_port: 2369 #雷达源端口号 + lidar_name: M10_PLUS #雷达选择:M10 M10_P M10_PLUS M10_GPS N10 L10 + angle_disable_min: 0.0 #角度裁剪开始值 + angle_disable_max: 90.0 #角度裁剪结束值 + min_range: 0.0 #雷达接收距离最小值 + max_range: 200.0 #雷达接收距离最大值 + use_gps_ts: false #雷达是否使用GPS授时 + scan_topic: /scan #设置激光数据topic名称 + interface_selection: net #接口选择:net 为网口,serial 为串口。 + serial_port_: /dev/ttyUSB0 #串口连接时的串口号 + high_reflection: false #M10_P雷达需填写该值,若不确定，请联系技术支持。 + compensation: true #M10系列是否使用角度补偿功能 + pubScan: true #是否发布scan话题 + pubPointCloud2: true #是否发布pointcloud2话题 + pointcloud_topic: /lslidar_point_cloud #设置激光数据topic名称 +# pcap: /home/ls/work/2211/M10_P_gps.pcap #雷达是否使用pcap包读取功能 +# in_file_name: /home/ls/1.txt #雷达是否使用txt文件读取功能 \ No newline at end of file diff --git a/Lslidar_ROS2_driver/lslidar_driver/rviz/lslidar.rviz b/Lslidar_ROS2_driver/lslidar_driver/rviz/lslidar.rviz new file mode 100644 index 0000000..5a3cdf5 --- /dev/null +++ b/Lslidar_ROS2_driver/lslidar_driver/rviz/lslidar.rviz @@ -0,0 +1,161 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /LaserScan1 + Splitter Ratio: 0.3441176414489746 + Tree Height: 617 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: scan + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: laser_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 3.635173797607422 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.8653978705406189 + Target Frame: + Value: Orbit (rviz) + Yaw: 3.095397710800171 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000156000002f4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002f4000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002f4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002f4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000292000002f400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1283 + X: 406 + Y: 152 diff --git a/Lslidar_ROS2_driver/lslidar_driver/src/input.cc b/Lslidar_ROS2_driver/lslidar_driver/src/input.cc new file mode 100644 index 0000000..cfa40a8 --- /dev/null +++ b/Lslidar_ROS2_driver/lslidar_driver/src/input.cc @@ -0,0 +1,398 @@ +#include "lslidar_driver/input.h" + +extern volatile sig_atomic_t flag; +namespace lslidar_driver +{ + static const size_t packet_size_input = 400; + //////////////////////////////////////////////////////////////////////// + // Input base class implementation + //////////////////////////////////////////////////////////////////////// + + /** @brief constructor + * + * @param private_nh ROS private handle for calling node. + * @param port UDP port number. + */ + Input::Input(rclcpp::Node *private_nh, uint16_t port) : private_nh_(private_nh), port_(port) { + npkt_update_flag_ = false; + cur_rpm_ = 0; + return_mode_ = 1; + devip_str_difop = std::string("192.168.1.200"); + devip_str_ = std::string("192.168.1.102"); + lidar_name = std::string("M10"); + add_multicast = false; + group_ip = std::string("224.1.1.2"); + UDP_PORT_NUMBER_DIFOP = 2369; + + + private_nh->declare_parameter("device_ip","192.168.1.102"); + private_nh->declare_parameter("device_ip_difop","192.168.1.200"); + private_nh->declare_parameter("add_multicast",false); + private_nh->declare_parameter("group_ip","224.1.1.2"); + private_nh->declare_parameter("difop_port",2369); + + + private_nh->get_parameter("lidar_name", lidar_name); + private_nh->get_parameter("device_ip", devip_str_); + private_nh->get_parameter("add_multicast", add_multicast); + private_nh->get_parameter("group_ip", group_ip); + private_nh->get_parameter("difop_port", UDP_PORT_NUMBER_DIFOP); + private_nh->get_parameter("device_ip_difop", devip_str_difop); + + if (!devip_str_.empty()) + RCLCPP_INFO(private_nh->get_logger(), "[driver][input] accepting packets from IP address: %s port: %d", + devip_str_.c_str(),port); + } + + /** @brief constructor + * + * @param private_nh ROS private handle for calling node. + * @param port UDP port number + */ + InputSocket::InputSocket(rclcpp::Node *private_nh, uint16_t port) : Input(private_nh, port) { + sockfd_ = -1; + + if (!devip_str_.empty()) { + inet_aton(devip_str_.c_str(), &devip_); + inet_aton(devip_str_difop.c_str(), &devip_difop); + } + + RCLCPP_INFO(private_nh_->get_logger(), "[driver][socket] Opening UDP socket: port %d", port); + sockfd_ = socket(PF_INET, SOCK_DGRAM, 0); + if (sockfd_ == -1) { + perror("socket"); // TODO: ROS_ERROR errno + return; + } + + int opt = 1; + if (setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (const void *) &opt, sizeof(opt))) { + perror("setsockopt error!\n"); + return; + } + + sockaddr_in my_addr; // my address information + memset(&my_addr, 0, sizeof(my_addr)); // initialize to zeros + my_addr.sin_family = AF_INET; // host byte order + my_addr.sin_port = htons(port); // port in network byte order + my_addr.sin_addr.s_addr = INADDR_ANY; // automatically fill in my IP + + if (bind(sockfd_, (sockaddr * ) & my_addr, sizeof(sockaddr)) == -1) { + perror("bind"); // TODO: ROS_ERROR errno + return; + } + + if (add_multicast) { + struct ip_mreq group; + group.imr_multiaddr.s_addr = inet_addr(group_ip.c_str()); + group.imr_interface.s_addr = htonl(INADDR_ANY); + + if (setsockopt(sockfd_, IPPROTO_IP, IP_ADD_MEMBERSHIP, (char *) &group, sizeof(group)) < 0) { + perror("Adding multicast group error "); + close(sockfd_); + exit(1); + } else + printf("Adding multicast group...OK.\n"); + } + if (fcntl(sockfd_, F_SETFL, O_NONBLOCK | FASYNC) < 0) { + perror("non-block"); + return; + } + } + + /** @brief destructor */ + InputSocket::~InputSocket(void) { + (void) close(sockfd_); + } + + void Input::UDP_difop() + { + sockaddr_in server_sai; + server_sai.sin_family = AF_INET; // IPV4 协议族 + server_sai.sin_port = htons(UDP_PORT_NUMBER_DIFOP); + server_sai.sin_addr.s_addr = inet_addr(devip_str_.c_str()); + for (int k = 0; k < 10; k++) + { + unsigned char data[188]= {0x00}; + data[0] = 0xA5; + data[1] = 0x5A; + data[2] = 0x55; + data[184] = 0x08; + data[185] = 0x01; + data[186] = 0xFA; + data[187] = 0xFB; + int rtn = sendto(sockfd_, data, 188, 0, (struct sockaddr *)&server_sai, sizeof(struct sockaddr)); + if (rtn < 0) printf("start scan error !\n"); + else return; + } + return; + } + + void Input::UDP_order(const std_msgs::msg::Int8 msg) + { + int i = msg.data; + sockaddr_in server_sai; + server_sai.sin_family = AF_INET; // IPV4 协议族 + server_sai.sin_port = htons(UDP_PORT_NUMBER_DIFOP); + server_sai.sin_addr.s_addr = inet_addr(devip_str_.c_str()); + int rtn = 0; + for (int k = 0; k < 10; k++) + { + unsigned char data[188]= {0x00}; + data[0] = 0xA5; + data[1] = 0x5A; + data[2] = 0x55; + data[186] = 0xFA; + data[187] = 0xFB; + if(lidar_name == "M10" || lidar_name == "M10_GPS" || lidar_name == "M10_P"){ + if (i <= 1){ //雷达启停 + data[184] = 0x01; + data[185] = char(i); + } + else if (i == 2){ //雷达点云不滤波 + data[181] = 0x0A; + data[184] = 0x06; + data[185] = 0x01; + } + else if (i == 3){ //雷达点云正常滤波 + data[181] = 0x0B; + data[184] = 0x06; + data[185] = 0x01; + } + else if (i == 4){ //雷达近距离滤波 + data[181] = 0x0C; + data[184] = 0x06; + data[185] = 0x01; + } + else if (i == 100){ //接收设备包 + data[184] = 0x08; + data[185] = 0x01; + } + else return; + } + else if (lidar_name == "M10_PLUS"){ + data[184] = 0x0A; + data[185] = 0x01; + if(i == 5) { + data[141] = 0x01; + data[142] = 0x2c; + } + else if(i == 6) { + data[141] = 0x01; + data[142] = 0x68; + } + else if(i == 8) { + data[141] = 0x01; + data[142] = 0xe0; + } + else if(i == 10) { + data[141] = 0x02; + data[142] = 0x58; + } + else if(i == 12) { + data[141] = 0x02; + data[142] = 0xd0; + } + else if(i == 15) { + data[141] = 0x03; + data[142] = 0x84; + } + else if(i == 20) { + data[141] = 0x04; + data[142] = 0xb0; + } + else if(i <= 1) { + data[184] = 0x01; + data[185] = char(i); + } + else if(i == 100) { //接收设备包 + data[184] = 0x08; + data[185] = 0x01; + } + else return; + } + else if(lidar_name == "N10"){ + if(i <= 1){ + data[185] = char(i); + data[184] = 0x01; + } + else if(i>=6 && i<=12){ + data[172] = char(i); + data[184] = 0x0a; + data[185] = 0X01; + } + else return; + } + rtn = sendto(sockfd_, data, 188, 0, (struct sockaddr *)&server_sai, sizeof(struct sockaddr)); + if (rtn < 0) + { + printf("start scan error !\n"); + } + else + { + if (i == 1) + usleep(3000000); + return; + } + } + return; + } + + + + int InputSocket::getPacket(lslidar_msgs::msg::LslidarPacket::UniquePtr &packet) + { + int q = 0; + struct pollfd fds[1]; + fds[0].fd = sockfd_; + fds[0].events = POLLIN; + static const int POLL_TIMEOUT = 2000; // one second (in msec) + + sockaddr_in sender_address{}; + socklen_t sender_address_len = sizeof(sender_address); + while (flag == 1) + { + // poll() until input available + do { + int retval = poll(fds, 1, POLL_TIMEOUT); + if (retval < 0) // poll() error? + { + if (errno != EINTR) + RCLCPP_ERROR(private_nh_->get_logger(), "[driver][socket] poll() error: %s", strerror(errno)); + return 0; + } + if (retval == 0) // poll() timeout? + { + RCLCPP_WARN(private_nh_->get_logger(), "lslidar poll() timeout, port: %d",port_); + return 0; + } + if ((fds[0].revents & POLLERR) || (fds[0].revents & POLLHUP) || (fds[0].revents & POLLNVAL)) // device error? + { + RCLCPP_ERROR(private_nh_->get_logger(),"poll() reports lslidar error"); + return 0; + } + } while ((fds[0].revents & POLLIN) == 0); + + // Receive packets that should now be available from the + // socket using a blocking read. + ssize_t nbytes = recvfrom(sockfd_, &packet->data[0], packet_size_input, 0, + (sockaddr *)&sender_address, &sender_address_len); + // ROS_DEBUG_STREAM("incomplete lslidar packet read: " + // << nbytes << " bytes"); + q = (int)nbytes; + if (nbytes < 0) + { + if (errno != EWOULDBLOCK) + { + perror("recvfail"); + RCLCPP_ERROR(private_nh_->get_logger(),"recvfail"); + return 1; + } + } + else if ((size_t)nbytes <= packet_size_input || (size_t)nbytes >= 50) + { + + // read successful, + // if packet is not from the lidar scanner we selected by IP, + // continue otherwise we are done + if (devip_str_ != "" && sender_address.sin_addr.s_addr != devip_.s_addr) + continue; + else + break; // done + } + + } + if (flag == 0) + { + abort(); + } + + return q; + } + InputPCAP::InputPCAP(rclcpp::Node *private_nh, uint16_t port, double packet_rate, std::string filename) : Input(private_nh, port), + packet_rate_(packet_rate), + filename_(filename) + { + pcap_ = NULL; + empty_ = true; + read_once_ = false; + read_fast_ = false; + repeat_delay_ = 0.0; + private_nh->get_parameter("read_once", read_once_); + private_nh->get_parameter("read_fast", read_fast_); + private_nh->get_parameter("repeat_delay", repeat_delay_); + + if (read_once_) + RCLCPP_WARN(private_nh_->get_logger(),"Read input file only once."); + if (read_fast_) + RCLCPP_WARN(private_nh_->get_logger(),"Read input file as quickly as possible."); + if (repeat_delay_ > 0.0) + RCLCPP_WARN(private_nh_->get_logger(),"Delay %.3f seconds before repeating input file.", repeat_delay_); + + RCLCPP_INFO(private_nh_->get_logger(),"Opening PCAP file %s",filename_.c_str()); + if ((pcap_ = pcap_open_offline(filename_.c_str(), errbuf_)) == NULL) + { + RCLCPP_WARN(private_nh_->get_logger(),"Error opening lslidar socket dump file."); + return; + } + std::stringstream filter; + if (devip_str_ != "") + { + filter << "src host " << devip_str_ << "&&"; + } + filter << "udp dst port " << port; + pcap_compile(pcap_, &pcap_packet_filter_, filter.str().c_str(), 1, PCAP_NETMASK_UNKNOWN); + } + + InputPCAP::~InputPCAP(void) + { + pcap_close(pcap_); + } + + int InputPCAP::getPacket(lslidar_msgs::msg::LslidarPacket::UniquePtr &pkt) + { + struct pcap_pkthdr *header; + const u_char *pkt_data; + while (flag == 1) + { + int res; + if ((res = pcap_next_ex(pcap_, &header, &pkt_data)) >= 0) + { + // skip packets not for the correct port and from the selected IP address + if (!devip_str_.empty() && (0 == pcap_offline_filter(&pcap_packet_filter_, header, pkt_data))) + continue; + + if (read_fast_ == false) + packet_rate_.sleep(); + mempcpy(&pkt->data[0], pkt_data + 42, packet_size_input); + empty_ = false; + return 0; + } + if (empty_) + { + RCLCPP_WARN(private_nh_->get_logger(),"Error %d reading lslidar packet: %s", res, pcap_geterr(pcap_)); + return -1; + } + if (read_once_) + { + RCLCPP_WARN(private_nh_->get_logger(),"end of file reached -- done reading."); + return -1; + } + if (repeat_delay_ > 0.0) + { + RCLCPP_WARN(private_nh_->get_logger(),"end of file reached -- delaying %.3f seconds.", repeat_delay_); + usleep(rint(repeat_delay_ * 1000000.0)); + } + RCLCPP_WARN(private_nh_->get_logger(),"replayding lsliar dump file"); + + pcap_close(pcap_); + pcap_ = pcap_open_offline(filename_.c_str(), errbuf_); + empty_ = true; + } + if (flag == 0) + { + abort(); + } + return 0; + } + +} // namespace diff --git a/Lslidar_ROS2_driver/lslidar_driver/src/lsiosr.cpp b/Lslidar_ROS2_driver/lslidar_driver/src/lsiosr.cpp new file mode 100644 index 0000000..d560b12 --- /dev/null +++ b/Lslidar_ROS2_driver/lslidar_driver/src/lsiosr.cpp @@ -0,0 +1,400 @@ +/******************************************************* +@company: Copyright (C) 2022, Leishen Intelligent System +@product: LSM10 and N10 +@filename: lsiosr.cpp +@brief: +@version: date: author: comments: +@v1.0 21-2-4 yao new +*******************************************************/ +#include "lslidar_driver/lsiosr.h" + +namespace lslidar_driver { + +LSIOSR * LSIOSR::instance(std::string name, int speed, int fd) +{ + static LSIOSR obj(name, speed, fd); + return &obj; +} + +LSIOSR::LSIOSR(std::string port, int baud_rate, int fd):port_(port), baud_rate_(baud_rate), fd_(fd) +{ + printf("port = %s, baud_rate = %d\n", port.c_str(), baud_rate); +} + +LSIOSR::~LSIOSR() +{ + close(); +} +/* 串口配置的函数 */ +int LSIOSR::setOpt(int nBits, uint8_t nEvent, int nStop) +{ + struct termios newtio, oldtio; + /*保存测试现有串口参数设置，在这里如果串口号等出错，会有相关的出错信息*/ + if (tcgetattr(fd_, &oldtio) != 0) + { + perror("SetupSerial 1"); + return -1; + } + bzero(&newtio, sizeof(newtio)); + /*步骤一，设置字符大小*/ + newtio.c_cflag |= CLOCAL; //如果设置，modem 的控制线将会被忽略。如果没有设置，则 open()函数会阻塞直到载波检测线宣告 modem 处于摘机状态为止。 + newtio.c_cflag |= CREAD; //使端口能读取输入的数据 + /*设置每个数据的位数*/ + switch (nBits) + { + case 7: + newtio.c_cflag |= CS7; + break; + case 8: + newtio.c_cflag |= CS8; + break; + } + /*设置奇偶校验位*/ + switch (nEvent) + { + case 'O': //奇数 + newtio.c_iflag |= (INPCK | ISTRIP); + newtio.c_cflag |= PARENB; //使能校验，如果不设PARODD则是偶校验 + newtio.c_cflag |= PARODD; //奇校验 + break; + case 'E': //偶数 + newtio.c_iflag |= (INPCK | ISTRIP); + newtio.c_cflag |= PARENB; + newtio.c_cflag &= ~PARODD; + break; + case 'N': //无奇偶校验位 + newtio.c_cflag &= ~PARENB; + break; + } + /*设置波特率*/ + switch (baud_rate_) + { + case 230400: + cfsetispeed(&newtio, B230400); + cfsetospeed(&newtio, B230400); + break; + case 460800: + cfsetispeed(&newtio, B460800); + cfsetospeed(&newtio, B460800); + break; + case 500000: + cfsetispeed(&newtio, B500000); + cfsetospeed(&newtio, B500000); + break; + case 921600: + cfsetispeed(&newtio, B921600); + cfsetospeed(&newtio, B921600); + break; + default: + cfsetispeed(&newtio, B460800); + cfsetospeed(&newtio, B460800); + break; + } + + /* + * 设置停止位 + * 设置停止位的位数， 如果设置，则会在每帧后产生两个停止位， 如果没有设置，则产生一个 + * 停止位。一般都是使用一位停止位。需要两位停止位的设备已过时了。 + * */ + if (nStop == 1) + newtio.c_cflag &= ~CSTOPB; + else if (nStop == 2) + newtio.c_cflag |= CSTOPB; + /*设置等待时间和最小接收字符*/ + newtio.c_cc[VTIME] = 0; + newtio.c_cc[VMIN] = 0; + /*处理未接收字符*/ + tcflush(fd_, TCIFLUSH); + /*激活新配置*/ + if ((tcsetattr(fd_, TCSANOW, &newtio)) != 0) + { + perror("serial set error"); + return -1; + } + + return 0; +} + +void LSIOSR::flushinput() { + tcflush(fd_, TCIFLUSH); +} + +/* 从串口中读取数据 */ +int LSIOSR::read(unsigned char *buffer, int length, int timeout) +{ + memset(buffer, 0, length); + + int totalBytesRead = 0; + int rc; + int unlink = 0; + unsigned char* pb = buffer; + + if (timeout > 0) + { + rc = waitReadable(timeout); + if (rc <= 0) + { + return (rc == 0) ? 0 : -1; + } + + int retry = 3; + while (length > 0) + { + rc = ::read(fd_, pb, (size_t)length); + + if (rc > 0) + { + length -= rc; + pb += rc; + totalBytesRead += rc; + + if (length == 0) + { + break; + } + } + else if (rc < 0) + { + printf("error \n"); + retry--; + if (retry <= 0) + { + break; + } + } + unlink++; + rc = waitReadable(20); + if(unlink > 10) + return -1; + + if (rc <= 0) + { + break; + } + } + } + else + { + rc = ::read(fd_, pb, (size_t)length); + + if (rc > 0) + { + totalBytesRead += rc; + } + else if ((rc < 0) && (errno != EINTR) && (errno != EAGAIN)) + { + printf("read error\n"); + return -1; + } + } + + return totalBytesRead; +} + +int LSIOSR::waitReadable(int millis) +{ + if (fd_ < 0) + { + return -1; + } + int serial = fd_; + + fd_set fdset; + struct timeval tv; + int rc = 0; + + while (millis > 0) + { + if (millis < 5000) + { + tv.tv_usec = millis % 1000 * 1000; + tv.tv_sec = millis / 1000; + + millis = 0; + } + else + { + tv.tv_usec = 0; + tv.tv_sec = 5; + + millis -= 5000; + } + + FD_ZERO(&fdset); + FD_SET(serial, &fdset); + + rc = select(serial + 1, &fdset, NULL, NULL, &tv); + if (rc > 0) + { + rc = (FD_ISSET(serial, &fdset)) ? 1 : -1; + break; + } + else if (rc < 0) + { + rc = -1; + break; + } + } + + return rc; +} + + +int LSIOSR::waitWritable(int millis) +{ + if (fd_ < 0) + { + return -1; + } + int serial = fd_; + + fd_set fdset; + struct timeval tv; + int rc = 0; + + while (millis > 0) + { + if (millis < 5000) + { + tv.tv_usec = millis % 1000 * 1000; + tv.tv_sec = millis / 1000; + + millis = 0; + } + else + { + tv.tv_usec = 0; + tv.tv_sec = 5; + + millis -= 5000; + } + + FD_ZERO(&fdset); + FD_SET(serial, &fdset); + + rc = select(serial + 1, NULL, &fdset, NULL, &tv); + if (rc > 0) + { + rc = (FD_ISSET(serial, &fdset)) ? 1 : -1; + break; + } + else if (rc < 0) + { + rc = -1; + break; + } + } + + return rc; +} + +/* 向串口中发送数据 */ +int LSIOSR::send(const char* buffer, int length, int timeout) +{ + if (fd_ < 0) + { + return -1; + } + + if ((buffer == 0) || (length <= 0)) + { + return -1; + } + + int totalBytesWrite = 0; + int rc; + char* pb = (char*)buffer; + + + if (timeout > 0) + { + rc = waitWritable(timeout); + if (rc <= 0) + { + return (rc == 0) ? 0 : -1; + } + + int retry = 3; + while (length > 0) + { + rc = write(fd_, pb, (size_t)length); + if (rc > 0) + { + length -= rc; + pb += rc; + totalBytesWrite += rc; + + if (length == 0) + { + break; + } + } + else + { + retry--; + if (retry <= 0) + { + break; + } + } + + rc = waitWritable(50); + if (rc <= 0) + { + break; + } + } + } + else + { + rc = write(fd_, pb, (size_t)length); + if (rc > 0) + { + totalBytesWrite += rc; + } + else if ((rc < 0) && (errno != EINTR) && (errno != EAGAIN)) + { + return -1; + } + } + + return totalBytesWrite; +} + +int LSIOSR::init() +{ + int error_code = 0; + + fd_ = open(port_.c_str(), O_RDWR|O_NOCTTY|O_NDELAY); + if (0 < fd_) + { + error_code = 0; + setOpt(DATA_BIT_8, PARITY_NONE, STOP_BIT_1);//设置串口参数 + //printf("open_port %s OK !\n", port_.c_str()); + } + else + { + error_code = -1; + } + + return error_code; +} + +int LSIOSR::close() +{ + ::close(fd_); + return 0; +} + +std::string LSIOSR::getPort() +{ + return port_; +} + +int LSIOSR::setPortName(std::string name) +{ + port_ = name; + return 0; +} + +} diff --git a/Lslidar_ROS2_driver/lslidar_driver/src/lslidar_driver.cc b/Lslidar_ROS2_driver/lslidar_driver/src/lslidar_driver.cc new file mode 100644 index 0000000..cdb2e08 --- /dev/null +++ b/Lslidar_ROS2_driver/lslidar_driver/src/lslidar_driver.cc @@ -0,0 +1,1383 @@ +/* + * This file is part of lslidar driver. + * + * The driver is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * The driver is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with the driver. If not, see . + */ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "lslidar_driver/lslidar_driver.h" +#include + +namespace lslidar_driver +{ + + static void my_hander(int sig) + { + printf("sig: %d", sig); + abort(); + } + LslidarDriver::LslidarDriver() : LslidarDriver(rclcpp::NodeOptions()) {} + LslidarDriver::LslidarDriver(const rclcpp::NodeOptions &options) : Node("lslidar_driver_node", options), diagnostics(this) + { + signal(SIGINT, my_hander); + + if (!this->initialize()) + RCLCPP_ERROR(this->get_logger(), "Could not initialize the driver..."); + else + RCLCPP_INFO(this->get_logger(), "Successfully initialize driver..."); + } + + LslidarDriver::~LslidarDriver() + { + return; + } + + bool LslidarDriver::loadParameters() + { + pubscan_thread_ = new boost::thread(boost::bind(&LslidarDriver::pubScanThread, this)); + interface_selection = std::string("net"); + frame_id = std::string("laser_link"); + scan_topic = std::string("/scan"); + lidar_name = std::string("M10"); + pointcloud_topic = std::string("/lslidar_point_cloud"); + is_start = true; + min_range = 0.3; + max_range = 100.0; + use_gps_ts = true; + compensation = true; + pubScan = true; + pubPointCloud2 = true; + angle_disable_min = 0.0; + angle_disable_max = 0.0; + + this->declare_parameter("lidar_name", "M10"); + this->declare_parameter("frame_id", "laser_link"); + this->declare_parameter("scan_topic", "/scan"); + this->declare_parameter("pointcloud_topic", "/lslidar_point_cloud"); + this->declare_parameter("min_range", 0.3); + this->declare_parameter("max_range", 100.0); + this->declare_parameter("use_gps_ts", false); + this->declare_parameter("high_reflection", false); + this->declare_parameter("compensation", false); + this->declare_parameter("pubScan", false); + this->declare_parameter("pubPointCloud2", false); + this->declare_parameter("angle_disable_min", 0.0); + this->declare_parameter("angle_disable_max", 0.0); + this->declare_parameter("interface_selection", "net"); + + this->get_parameter("lidar_name", lidar_name); + this->get_parameter("frame_id", frame_id); + this->get_parameter("high_reflection", high_reflection); + this->get_parameter("scan_topic", scan_topic); + this->get_parameter("min_range", min_range); + this->get_parameter("max_range", max_range); + this->get_parameter("use_gps_ts", use_gps_ts); + this->get_parameter("compensation", compensation); + this->get_parameter("pointcloud_topic", pointcloud_topic); + this->get_parameter("pubScan", pubScan); + this->get_parameter("pubPointCloud2", pubPointCloud2); + this->get_parameter("angle_disable_min", angle_disable_min); + this->get_parameter("angle_disable_max", angle_disable_max); + this->get_parameter("interface_selection", interface_selection); + while (angle_disable_min < 0) + angle_disable_min += 360; + while (angle_disable_max < 0) + angle_disable_max += 360; + while (angle_disable_min > 360) + angle_disable_min -= 360; + while (angle_disable_max > 360) + angle_disable_max -= 360; + if (angle_disable_max == angle_disable_min) + { + angle_able_min = 0; + angle_able_max = 360; + } + else + { + if (angle_disable_min < angle_disable_max && angle_disable_min != 0.0) + { + angle_able_min = angle_disable_max; + angle_able_max = angle_disable_min + 360; + } + if (angle_disable_min < angle_disable_max && angle_disable_min == 0.0) + { + angle_able_min = angle_disable_max; + angle_able_max = 360; + } + if (angle_disable_min > angle_disable_max) + { + angle_able_min = angle_disable_max; + angle_able_max = angle_disable_min; + } + } + count_num = 0; + + scan_points_.resize(6000); + + if (lidar_name == "M10") + { + use_gps_ts = false; + PACKET_SIZE = 92; + package_points = 42; + data_bits_start = 6; + degree_bits_start = 2; + rpm_bits_start = 4; + baud_rate_ = 460800; + points_size_ = 1008; + } + else if (lidar_name == "M10_P") + { + PACKET_SIZE = 160; + package_points = 70; + data_bits_start = 8; + degree_bits_start = 4; + rpm_bits_start = 6; + baud_rate_ = 500000; + points_size_ = 2000; + } + else if (lidar_name == "M10_PLUS") + { + PACKET_SIZE = 104; + package_points = 41; + data_bits_start = 8; + degree_bits_start = 4; + rpm_bits_start = 6; + points_size_ = 5000; + baud_rate_ = 921600; + } + else if (lidar_name == "M10_GPS") + { + PACKET_SIZE = 102; + package_points = 42; + data_bits_start = 6; + degree_bits_start = 2; + rpm_bits_start = 4; + baud_rate_ = 460800; + points_size_ = 1008; + } + else if (lidar_name == "N10") + { + PACKET_SIZE = 58; + package_points = 16; + data_bits_start = 7; + degree_bits_start = 5; + end_degree_bits_start = 55; + baud_rate_ = 230400; + points_size_ = 2000; + use_gps_ts = false; + compensation = false; + } + else if (lidar_name == "M10_DOUBLE") + { + PACKET_SIZE = 300; + package_points = 70; + data_bits_start = 8; + degree_bits_start = 4; + rpm_bits_start = 6; + points_size_ = 3000; + baud_rate_ = 921600; + } + else if (lidar_name == "N10_P") + { + PACKET_SIZE = 108; + package_points = 16; + data_bits_start = 7; + degree_bits_start = 5; + end_degree_bits_start = 105; + baud_rate_ = 460800; + points_size_ = 2000; + use_gps_ts = false; + compensation = false; + } + else if (lidar_name == "L10") + { + PACKET_SIZE = 58; + package_points = 16; + data_bits_start = 7; + degree_bits_start = 5; + end_degree_bits_start = 55; + baud_rate_ = 230400; + points_size_ = 2000; + use_gps_ts = false; + compensation = false; + } + RCLCPP_INFO_STREAM(this->get_logger(), "Lidar is " << lidar_name.c_str()); + + if (pubScan) + scan_pub = this->create_publisher(scan_topic, 10); + if (pubPointCloud2) + point_cloud_pub = this->create_publisher(pointcloud_topic, 10); + difop_switch = this->create_subscription("lslidar_order", 1, std::bind(&LslidarDriver::lidar_order, this, std::placeholders::_1)); // 转速输入 + return true; + } + + void LslidarDriver::lidar_difop() + { + if (lidar_name == "L10" || lidar_name == "N10" || lidar_name == "N10_P") + return; + if (interface_selection == "net") + msop_input_->UDP_difop(); + else + { + for (int k = 0; k < 10; k++) + { + unsigned char data[188] = {0x00}; + data[0] = 0xA5; + data[1] = 0x5A; + data[2] = 0x55; + data[184] = 0x08; + data[185] = 0x01; + data[186] = 0xFA; + data[187] = 0xFB; + int rtn = serial_->send((const char *)data, 188); + if (rtn < 0) + printf("start scan error !\n"); + else + return; + } + } + return; + } + + void LslidarDriver::lidar_order(const std_msgs::msg::Int8::SharedPtr msg) + { + if (lidar_name == "L10") + return; + int i = msg->data; + if (i == 0) + is_start = false; + else + is_start = true; + if (interface_selection == "net") + msop_input_->UDP_order(*msg); + else + { + int i = msg->data; + for (int k = 0; k < 10; k++) + { + int rtn; + unsigned char data[188] = {0x00}; + data[0] = 0xA5; + data[1] = 0x5A; + data[2] = 0x55; + data[186] = 0xFA; + data[187] = 0xFB; + + if (lidar_name == "M10" || lidar_name == "M10_GPS" || lidar_name == "M10_P" || lidar_name == "M10_DOUBLE") + { + if (i <= 1) + { // 雷达启停 + data[184] = 0x01; + data[185] = char(i); + } + else if (i == 2) + { // 雷达点云不滤波 + data[181] = 0x0A; + data[184] = 0x06; + if (is_start) + data[185] = 0x01; + } + else if (i == 3) + { // 雷达点云正常滤波 + data[181] = 0x0B; + data[184] = 0x06; + if (is_start) + data[185] = 0x01; + } + else if (i == 4) + { // 雷达近距离滤波 + data[181] = 0x0C; + data[184] = 0x06; + if (is_start) + data[185] = 0x01; + } + else if (i == 100) + { // 接收设备包 + data[184] = 0x08; + data[185] = 0x01; + } + else + return; + } + else if (lidar_name == "M10_PLUS") + { + data[184] = 0x0A; + data[185] = 0x01; + if (i == 5) + { + data[141] = 0x01; + data[142] = 0x2c; + } + else if (i == 6) + { + data[141] = 0x01; + data[142] = 0x68; + } + else if (i == 8) + { + data[141] = 0x01; + data[142] = 0xe0; + } + else if (i == 10) + { + data[141] = 0x02; + data[142] = 0x58; + } + else if (i == 12) + { + data[141] = 0x02; + data[142] = 0xd0; + } + else if (i == 15) + { + data[141] = 0x03; + data[142] = 0x84; + } + else if (i == 20) + { + data[141] = 0x04; + data[142] = 0xb0; + } + else if (i <= 1) + { + data[184] = 0x01; + data[185] = char(i); + } + else if (i == 100) // 接收设备包 + { + data[184] = 0x08; + data[185] = 0x01; + } + else + return; + } + else if (lidar_name == "N10" || lidar_name == "N10_P") + { + if (i <= 1) + { + data[185] = char(i); + data[184] = 0x01; + } + else if (i >= 6 && i <= 12) + { + data[172] = char(i); + data[184] = 0x0a; + data[185] = 0X01; + } + else + return; + } + rtn = serial_->send((const char *)data, 188); + if (rtn < 0) + printf("start scan error !\n"); + else + { + if (i == 1) + usleep(1000000); // 1.0s + if (i == 0) + is_start = false; + if (i == 1) + is_start = true; + return; + } + } + return; + } + } + + void LslidarDriver::open_serial() + { + diagnostics.setHardwareID("Lslidar"); + int code = 0; + serial_port_ = std::string("/dev/ttyUSB0"); + this->declare_parameter("serial_port_", "/dev/ttyUSB0"); + this->get_parameter("serial_port_", serial_port_); + serial_ = LSIOSR::instance(serial_port_, baud_rate_); + code = serial_->init(); + if (code != 0) + { + printf("open_port %s ERROR !\n", serial_port_.c_str()); + rclcpp::shutdown(); + exit(0); + } + printf("open_port %s OK !\n", serial_port_.c_str()); + } + + bool LslidarDriver::createRosIO() + { + UDP_PORT_NUMBER = 2368; + this->declare_parameter("msop_port", 2368); + this->get_parameter("msop_port", UDP_PORT_NUMBER); + RCLCPP_INFO_STREAM(this->get_logger(), "Opening UDP socket: port " << UDP_PORT_NUMBER); + dump_file = std::string(""); + this->declare_parameter("pcap", ""); + this->get_parameter("pcap", dump_file); + // ROS diagnostics + diagnostics.setHardwareID("Lslidar"); + + const double diag_freq = 12 * 24; + diag_max_freq = diag_freq; + diag_min_freq = diag_freq; + RCLCPP_INFO(this->get_logger(), "expected frequency: %.3f (Hz)", diag_freq); + + using namespace diagnostic_updater; + diag_topic.reset(new TopicDiagnostic( + "lslidar_packets", diagnostics, + FrequencyStatusParam(&diag_min_freq, &diag_max_freq, 0.1, 10), + TimeStampStatusParam())); + + int hz = 10; + if (lidar_name == "M10_P") + hz = 12; + else if (lidar_name == "M10_PLUS") + hz = 20; + + double packet_rate = hz * 24; + if (dump_file != "") + { + msop_input_.reset(new lslidar_driver::InputPCAP(this, UDP_PORT_NUMBER, packet_rate, dump_file)); + } + else + { + msop_input_.reset(new lslidar_driver::InputSocket(this, UDP_PORT_NUMBER)); + } + + // Output + return true; + } + + int LslidarDriver::getScan(std::vector &points, rclcpp::Time &scan_time, float &scan_duration) + { + boost::unique_lock lock(mutex_); + points.assign(scan_points_bak_.begin(), scan_points_bak_.end()); + scan_time = pre_time_; + scan_duration = time_.seconds() - pre_time_.seconds(); + return 1; + } + + uint64_t LslidarDriver::get_gps_stamp(struct tm t) + { + + uint64_t ptime = static_cast(timegm(&t)); + return ptime; + } + + bool LslidarDriver::initialize() + { + if (!loadParameters()) + { + RCLCPP_ERROR(this->get_logger(), "Cannot load all required ROS parameters..."); + return false; + } + if (interface_selection == "net") + { + if (!createRosIO()) + { + RCLCPP_ERROR(this->get_logger(), "Cannot create all ROS IO..."); + return false; + } + } + else + { + in_file_name = std::string(""); + this->declare_parameter("in_file_name", ""); + this->get_parameter("in_file_name", in_file_name); + if (in_file_name == "") + open_serial(); + else + { + RCLCPP_INFO_STREAM(this->get_logger(), "Opening txt file " << in_file_name.c_str()); + std::ifstream file_reader(in_file_name); + if (!file_reader.is_open()) + { + RCLCPP_ERROR(this->get_logger(), "Cannot open the file"); + return false; + } + } + } + RCLCPP_INFO(this->get_logger(), "Initialised lslidar without error"); + return true; + } + + void LslidarDriver::recvThread_crc(int &count, int &link_time) + { + if (count <= 0) + link_time++; + else + link_time = 0; + + if (link_time > 150) + { + serial_->close(); + int ret = serial_->init(); + if (ret < 0) + { + RCLCPP_ERROR(this->get_logger(), "serial open fail"); + usleep(200000); + } + link_time = 0; + } + } + + int LslidarDriver::receive_data(unsigned char *packet_bytes) + { + int link_time = 0; + int len_H = 0; + int len_L = 0; + int len = 0; + int count_2 = 0; + int count = 0; + while (count <= 0) + { + count = serial_->read(packet_bytes, 1); + LslidarDriver::recvThread_crc(count, link_time); + } + if (packet_bytes[0] != 0xA5) + return 0; + + while (count_2 <= 0) + { + count_2 = serial_->read(packet_bytes + count, 1); + if (count_2 >= 0) + count += count_2; + LslidarDriver::recvThread_crc(count_2, link_time); + } + + count_2 = 0; + if (packet_bytes[1] != 0x5A) + return 0; + while (count_2 <= 0) + { + count_2 = serial_->read(packet_bytes + count, 2); + if (count_2 >= 0) + count += count_2; + LslidarDriver::recvThread_crc(count_2, link_time); + } + + count_2 = 0; + + if (lidar_name == "M10") + len = 92; + else if (lidar_name == "M10_GPS") + len = 102; + else if (lidar_name == "N10_P") + len = 108; + else if (lidar_name == "N10" || lidar_name == "L10") + len = packet_bytes[2]; + else + { + len_H = packet_bytes[2]; + len_L = packet_bytes[3]; + len = len_H * 256 + len_L; + } + if (lidar_name == "M10" || lidar_name == "M10_DOUBLE" || lidar_name == "M10_GPS" || lidar_name == "M10_P" || lidar_name == "M10_PLUS") + { + if (packet_bytes[2] == 0x55 && packet_bytes[3] == 0x00) + len = 188; + } + while (count < len) + { + count_2 = serial_->read(packet_bytes + count, len - count); + if (count_2 >= 0) + count += count_2; + LslidarDriver::recvThread_crc(count_2, link_time); + } + if (lidar_name == "N10" || lidar_name == "L10" || lidar_name == "N10_P") + { + if (packet_bytes[PACKET_SIZE - 1] != N10_CalCRC8(packet_bytes, PACKET_SIZE - 1)) + return 0; + } + return len; + } + + uint8_t LslidarDriver::N10_CalCRC8(unsigned char *p, int len) + { + uint8_t crc = 0; + int sum = 0; + + for (int i = 0; i < len; i++) + { + sum += uint8_t(p[i]); + } + crc = sum & 0xff; + return crc; + } + + void LslidarDriver::difop_processing(unsigned char *packet_bytes) // 处理设备包的数据 + { + int s = packet_bytes[173]; + int z = packet_bytes[174]; + int degree_temp = s & 0x7F; + int sign_temp = s & 0x80; + degree_compensation = double(degree_temp * 256 + z) / 100.f; + if (sign_temp) + degree_compensation = -degree_compensation; + first_compensation = false; + printf("degree_compensation = %f\n", degree_compensation); + return; + } + + void LslidarDriver::data_processing(unsigned char *packet_bytes, int len) // 处理每一包的数据 + { + double degree; + double end_degree; + double degree_interval = 15.0; + boost::posix_time::ptime t1, t2; + t1 = boost::posix_time::microsec_clock::universal_time(); + + int s = packet_bytes[degree_bits_start]; + int z = packet_bytes[degree_bits_start + 1]; + + degree = (s * 256 + z) / 100.f + degree_compensation; + degree = (degree < 0) ? degree + 360 : degree; + degree = (degree > 360) ? degree - 360 : degree; + if (lidar_name == "N10" || lidar_name == "L10") + { + int s_e = packet_bytes[end_degree_bits_start]; + int z_e = packet_bytes[end_degree_bits_start + 1]; + + end_degree = (s_e * 256 + z_e) / 100.f; + end_degree = (end_degree > 360) ? end_degree - 360 : end_degree; + + if (degree > end_degree) + degree_interval = end_degree + 360 - degree; + else + degree_interval = end_degree - degree; + } + + // boost::unique_lock lock(mutex_); + if (lidar_name == "M10_PLUS" || lidar_name == "M10_P") + { + PACKET_SIZE = len; + package_points = (PACKET_SIZE - 20) / 2; + } + int invalidValue = 0; + int point_len = 2; + if (lidar_name == "N10" || lidar_name == "L10") + point_len = 3; + + if (lidar_name == "M10_GPS" || lidar_name == "M10") + { + int err_data_84 = packet_bytes[84]; + int err_data_85 = packet_bytes[85]; + if ((err_data_84 * 256 + err_data_85) == 0xFFFF || packet_bytes[86] >= 0xF5) + { + packet_bytes[86] = 0xFF; + packet_bytes[87] = 0xFF; + } + } + + for (int num = 0; num < point_len * package_points; num += point_len) + { + int s = packet_bytes[num + data_bits_start]; + int z = packet_bytes[num + data_bits_start + 1]; + if ((s * 256 + z) == 0xFFFF) + invalidValue++; + } + + if (use_gps_ts && lidar_name != "N10") + { + pTime.tm_year = packet_bytes[PACKET_SIZE - 12] + 2000 - 1900; // x+2000 + pTime.tm_mon = packet_bytes[PACKET_SIZE - 11] - 1; // 1-12 + pTime.tm_mday = packet_bytes[PACKET_SIZE - 10]; // 1-31 + pTime.tm_hour = packet_bytes[PACKET_SIZE - 9]; // 0-23 + pTime.tm_min = packet_bytes[PACKET_SIZE - 8]; // 0-59 + pTime.tm_sec = packet_bytes[PACKET_SIZE - 7]; // 0-59 + sub_second = (packet_bytes[PACKET_SIZE - 6] * 256 + packet_bytes[PACKET_SIZE - 5]) * 1000000 + (packet_bytes[PACKET_SIZE - 4] * 256 + packet_bytes[PACKET_SIZE - 3]) * 1000; + sweep_end_time_gps = get_gps_stamp(pTime); + sweep_end_time_hardware = sub_second % 1000000000; + } + invalidValue = package_points - invalidValue; + if (lidar_name == "N10" || lidar_name == "L10") + invalidValue--; + if (invalidValue <= 1) + { + delete packet_bytes; + return; + } + + for (int num = 0; num < package_points; num++) + { + int s = packet_bytes[num * point_len + data_bits_start]; + int z = packet_bytes[num * point_len + data_bits_start + 1]; + int y = 0; + if (lidar_name == "N10" || lidar_name == "L10") + y = packet_bytes[num * point_len + data_bits_start + 2]; + int dist_temp = s & 0x7F; + int inten_temp = s & 0x80; + + if ((s * 256 + z) != 0xFFFF) + { + if (lidar_name == "N10" || lidar_name == "L10") + { + scan_points_[idx].range = double(s * 256 + (z)) / 1000.f; + scan_points_[idx].intensity = int(y); + } + else if ((lidar_name == "M10_P" || lidar_name == "M10_PLUS") && !high_reflection) + { + scan_points_[idx].range = double(s * 256 + (z)) / 1000.f; + scan_points_[idx].intensity = 0; + } + else + { + scan_points_[idx].range = double(dist_temp * 256 + (z)) / 1000.f; + if (inten_temp) + scan_points_[idx].intensity = 255; + else + scan_points_[idx].intensity = 0; + } + if ((degree + (degree_interval / invalidValue * num)) > 360) + scan_points_[idx].degree = degree + (degree_interval / invalidValue * num) - 360; + else + scan_points_[idx].degree = degree + (degree_interval / invalidValue * num); + } + else + continue; + + if ((scan_points_[idx].degree < last_degree && scan_points_[idx].degree < 5 && last_degree > 355) || idx >= points_size_) + { + last_degree = scan_points_[idx].degree; + count_num = idx; + idx = 0; + for (long unsigned int k = 0; k < scan_points_.size(); k++) + { + if (scan_points_[k].range < min_range || scan_points_[k].range > max_range) + scan_points_[k].range = 0; + } + boost::unique_lock lock(mutex_); + scan_points_bak_.resize(scan_points_.size()); + scan_points_bak_.assign(scan_points_.begin(), scan_points_.end()); + for (long unsigned int k = 0; k < scan_points_.size(); k++) + { + scan_points_[k].range = 0; + scan_points_[k].degree = 0; + scan_points_[k].intensity = 0; + } + pre_time_ = time_; + lock.unlock(); + pubscan_cond_.notify_one(); + time_ = get_clock()->now(); + } + else + { + last_degree = scan_points_[idx].degree; + idx++; + } + } + packet_bytes = {0x00}; + if (packet_bytes) + { + packet_bytes = NULL; + delete packet_bytes; + } + } + + void LslidarDriver::data_processing_2(unsigned char *packet_bytes, int len) // 处理每一包的数据 + { + double degree; + double end_degree; + double degree_interval = 15.0; + boost::posix_time::ptime t1, t2; + t1 = boost::posix_time::microsec_clock::universal_time(); + + int s = packet_bytes[degree_bits_start]; + int z = packet_bytes[degree_bits_start + 1]; + + degree = (s * 256 + z) / 100.f + degree_compensation; + degree = (degree < 0) ? degree + 360 : degree; + degree = (degree > 360) ? degree - 360 : degree; + if (lidar_name == "N10_P") + { + int s_e = packet_bytes[end_degree_bits_start]; + int z_e = packet_bytes[end_degree_bits_start + 1]; + + end_degree = (s_e * 256 + z_e) / 100.f; + end_degree = (end_degree > 360) ? end_degree - 360 : end_degree; + + if (degree > end_degree) + degree_interval = end_degree + 360 - degree; + else + degree_interval = end_degree - degree; + } + + // boost::unique_lock lock(mutex_); + if (lidar_name == "M10_DOUBLE") + { + PACKET_SIZE = len; + package_points = (PACKET_SIZE - 20) / 4; + } + int invalidValue = 0; + int point_len = 4; + if (lidar_name == "N10_P") + point_len = 6; + + for (int num = 0; num < point_len * package_points; num += point_len) + { + int s = packet_bytes[num + data_bits_start]; + int z = packet_bytes[num + data_bits_start + 1]; + if ((s * 256 + z) == 0xFFFF) + invalidValue++; + } + + if (use_gps_ts) + { + pTime.tm_year = packet_bytes[PACKET_SIZE - 12] + 2000 - 1900; // x+2000 + pTime.tm_mon = packet_bytes[PACKET_SIZE - 11] - 1; // 1-12 + pTime.tm_mday = packet_bytes[PACKET_SIZE - 10]; // 1-31 + pTime.tm_hour = packet_bytes[PACKET_SIZE - 9]; // 0-23 + pTime.tm_min = packet_bytes[PACKET_SIZE - 8]; // 0-59 + pTime.tm_sec = packet_bytes[PACKET_SIZE - 7]; // 0-59 + sub_second = (packet_bytes[PACKET_SIZE - 6] * 256 + packet_bytes[PACKET_SIZE - 5]) * 1000000 + (packet_bytes[PACKET_SIZE - 4] * 256 + packet_bytes[PACKET_SIZE - 3]) * 1000; + sweep_end_time_gps = get_gps_stamp(pTime); + sweep_end_time_hardware = sub_second % 1000000000; + } + invalidValue = package_points - invalidValue; + if (lidar_name == "N10_P") + invalidValue--; + if (invalidValue <= 1) + { + delete packet_bytes; + return; + } + + for (int num = 0; num < package_points; num++) + { + int s = packet_bytes[num * point_len + data_bits_start]; + int z = packet_bytes[num * point_len + data_bits_start + 1]; + int y = 0; + if (lidar_name == "N10_P") + y = packet_bytes[num * point_len + data_bits_start + 2]; + + if ((s * 256 + z) != 0xFFFF) + { + scan_points_[idx].range = double(s * 256 + (z)) / 1000.f; + if (lidar_name == "N10_P") + scan_points_[idx].intensity = int(y); + else + scan_points_[idx].intensity = 0; + s = packet_bytes[num * point_len + data_bits_start + point_len / 2]; + z = packet_bytes[num * point_len + data_bits_start + point_len / 2 + 1]; + if (lidar_name == "N10_P") + y = packet_bytes[num * point_len + data_bits_start + point_len / 2 + 2]; + + scan_points_[idx + 3000].range = double(s * 256 + (z)) / 1000.f; + if (lidar_name == "N10_P") + scan_points_[idx + 3000].intensity = int(y); + else + scan_points_[idx + 3000].intensity = 0; + + if ((degree + (degree_interval / invalidValue * num)) > 360) + scan_points_[idx].degree = degree + (degree_interval / invalidValue * num) - 360; + else + scan_points_[idx].degree = degree + (degree_interval / invalidValue * num); + } + else + continue; + if (((scan_points_[idx].degree < last_degree && scan_points_[idx].degree < 5 && last_degree > 355) || idx >= points_size_) && idx > 10) + { + last_degree = scan_points_[idx].degree; + count_num = idx; + idx = 0; + for (int k = 0; k < count_num; k++) + { + if (angle_able_max > 360) + { + if ((360 - scan_points_[k].degree) > (angle_able_max - 360) && (360 - scan_points_[k].degree) < angle_able_min) + { + scan_points_[k].range = 0; + scan_points_[k + 3000].range = 0; + } + } + else + { + if ((360 - scan_points_[k].degree) > angle_able_max || (360 - scan_points_[k].degree) < angle_able_min) + { + scan_points_[k].range = 0; + scan_points_[k + 3000].range = 0; + } + } + if (scan_points_[k].range < min_range || scan_points_[k].range > max_range) + scan_points_[k].range = 0; + if (scan_points_[k + 3000].range < min_range || scan_points_[k + 3000].range > max_range) + scan_points_[k + 3000].range = 0; + } + boost::unique_lock lock(mutex_); + scan_points_bak_.resize(scan_points_.size()); + scan_points_bak_.assign(scan_points_.begin(), scan_points_.end()); + for (long unsigned int k = 0; k < scan_points_.size(); k++) + { + scan_points_[k].range = 0; + scan_points_[k].degree = 0; + scan_points_[k].intensity = 0; + } + pre_time_ = time_; + lock.unlock(); + pubscan_cond_.notify_one(); + time_ = get_clock()->now(); + } + else + { + last_degree = scan_points_[idx].degree; + idx++; + } + } + packet_bytes = {0x00}; + if (packet_bytes) + { + packet_bytes = NULL; + delete packet_bytes; + } + } + + void LslidarDriver::pubScanThread() + { + bool wait_for_wake = true; + boost::unique_lock lock(pubscan_mutex_); + + while (rclcpp::ok()) + { + + while (wait_for_wake) + { + pubscan_cond_.wait(lock); + wait_for_wake = false; + } + if (lidar_name == "N10_P" || lidar_name == "M10_DOUBLE") + { + if (pubScan) + { + auto scan = sensor_msgs::msg::LaserScan::UniquePtr(new sensor_msgs::msg::LaserScan()); + int scan_num = count_num * 2; + std::vector points; + rclcpp::Time start_time; + float scan_time; + this->getScan(points, start_time, scan_time); + scan->header.frame_id = frame_id; + if (use_gps_ts) + { + scan->header.stamp = rclcpp::Time(sweep_end_time_gps, sweep_end_time_hardware); + } + else + { + scan->header.stamp = this->now(); // timestamp will obtained from sweep data stamp + } + + scan->angle_min = 0; + scan->angle_max = 2 * M_PI; + scan->angle_increment = 2 * M_PI / (double)(count_num); + scan->range_min = min_range; + scan->range_max = max_range; + scan->ranges.reserve(scan_num); + scan->ranges.assign(scan_num, std::numeric_limits::infinity()); + scan->intensities.reserve(scan_num); + scan->intensities.assign(scan_num, std::numeric_limits::infinity()); + // scan->scan_time = scan_time; + // scan->time_increment = scan_time / (double)(count_num); + + for (int k = 0; k < scan_num; k++) + { + scan->ranges[k] = std::numeric_limits::infinity(); + scan->intensities[k] = 0; + } + + for (int i = 0; i < count_num; i++) + { + int point_idx = round((360 - points[i].degree) * count_num / 360); + if (points[i].range == 0.0) + { + scan->ranges[point_idx] = std::numeric_limits::infinity(); + scan->intensities[point_idx] = 0; + } + else + { + double dist = points[i].range; + scan->ranges[point_idx] = (float)dist; + scan->intensities[point_idx] = points[i].intensity; + } + if (points[i + 3000].range == 0.0) + { + scan->ranges[point_idx + count_num] = std::numeric_limits::infinity(); + scan->intensities[point_idx + count_num] = 0; + } + else + { + double dist = points[i+3000].range; + scan->ranges[point_idx + count_num] = (float)dist; + scan->intensities[point_idx + count_num] = points[i + 3000].intensity; + } + } + scan_pub->publish(std::move(scan)); + } + if (pubPointCloud2) + { + std::vector points; + rclcpp::Time start_time; + float scan_time; + this->getScan(points, start_time, scan_time); + VPointCloud::Ptr point_cloud(new VPointCloud()); + if (use_gps_ts) + { + start_time = rclcpp::Time(sweep_end_time_gps, sweep_end_time_hardware); + } + double timestamp = start_time.seconds(); + point_cloud->header.stamp = static_cast(timestamp * 1e6); + point_cloud->header.frame_id = frame_id; + point_cloud->height = 1; + // printf("now = %f\n",timestamp); + for (uint16_t i = 0; i < count_num; i++) + { + // printf("degree = %f\n",points[i].degree); + double degree = 360.0 - points[i].degree; + bool pass_point = false; + if (angle_able_max < 360) + { + if (degree < angle_able_min || degree > angle_able_max) + pass_point = true; + } + else + { + if (degree < angle_able_min && degree > (angle_able_max - 360)) + pass_point = true; + } + if (points[i].range < 0.001) + pass_point = true; + if (!pass_point) + { + // printf("degree = %f\n",degree); + // printf("angle_able_min = %f\nangle_able_max=%f\n",angle_able_min,angle_able_max); + VPoint point; + int point_idx = round(degree * count_num / 360); + point.timestamp = timestamp - point_idx * (scan_time / count_num); + // printf("timestamp = %f\n",point.timestamp); + point.x = points[i].range * cos(M_PI / 180 * points[i].degree); + point.y = -points[i].range * sin(M_PI / 180 * points[i].degree); + point.z = 0; + point.intensity = points[i].intensity; + point_cloud->points.push_back(point); + ++point_cloud->width; + } + if (points[i + 3000].range < 0.001) + pass_point = true; + if (!pass_point) + { + // printf("degree = %f\n",degree); + // printf("angle_able_min = %f\nangle_able_max=%f\n",angle_able_min,angle_able_max); + VPoint point; + int point_idx = round(degree * count_num / 360); + point.timestamp = timestamp - point_idx * (scan_time / count_num); + // printf("timestamp = %f\n",point.timestamp); + point.x = points[i + 3000].range * cos(M_PI / 180 * points[i].degree); + point.y = -points[i + 3000].range * sin(M_PI / 180 * points[i].degree); + point.z = 0; + point.intensity = points[i + 3000].intensity; + point_cloud->points.push_back(point); + ++point_cloud->width; + } + } + sensor_msgs::msg::PointCloud2 pc_msg; + pcl::toROSMsg(*point_cloud, pc_msg); + point_cloud_pub->publish(pc_msg); + } + } + else + { + if (pubScan) + { + auto scan = sensor_msgs::msg::LaserScan::UniquePtr(new sensor_msgs::msg::LaserScan()); + int scan_num = ceil((angle_able_max - angle_able_min) / 360 * count_num) + 1; + std::vector points; + rclcpp::Time start_time; + float scan_time; + this->getScan(points, start_time, scan_time); + scan->header.frame_id = frame_id; + if (use_gps_ts) + { + scan->header.stamp = rclcpp::Time(sweep_end_time_gps, sweep_end_time_hardware); + } + else + { + scan->header.stamp = this->now(); // timestamp will obtained from sweep data stamp + } + + if (angle_able_max > 360) + { + scan->angle_min = 2 * M_PI * (angle_able_min - 360) / 360; + scan->angle_max = 2 * M_PI * (angle_able_max - 360) / 360; + } + else + { + scan->angle_min = 2 * M_PI * angle_able_min / 360; + scan->angle_max = 2 * M_PI * angle_able_max / 360; + } + scan->angle_increment = 2 * M_PI / (double)(count_num - 1); + + scan->range_min = min_range; + scan->range_max = max_range; + scan->ranges.reserve(scan_num); + scan->ranges.assign(scan_num, std::numeric_limits::infinity()); + scan->intensities.reserve(scan_num); + scan->intensities.assign(scan_num, std::numeric_limits::infinity()); + scan->scan_time = scan_time; + scan->time_increment = scan_time / (double)(count_num - 1); + + int start_num = floor(angle_able_min * count_num / 360); + int end_num = floor(angle_able_max * count_num / 360); + + for (int i = 0; i < count_num; i++) + { + int point_idx = round((360 - points[i].degree) * count_num / 360); + if (point_idx < (end_num - count_num)) + point_idx += count_num; + point_idx = point_idx - start_num; + if (point_idx < 0 || point_idx >= scan_num) + continue; + if (points[i].range == 0.0) + { + scan->ranges[point_idx] = std::numeric_limits::infinity(); + } + else + { + double dist = points[i].range; + scan->ranges[point_idx] = (float)dist; + } + scan->intensities[point_idx] = points[i].intensity; + } + scan_pub->publish(std::move(scan)); + } + if (pubPointCloud2) + { + std::vector points; + rclcpp::Time start_time; + float scan_time; + this->getScan(points, start_time, scan_time); + VPointCloud::Ptr point_cloud(new VPointCloud()); + if (use_gps_ts) + { + start_time = rclcpp::Time(sweep_end_time_gps, sweep_end_time_hardware); + } + double timestamp = start_time.seconds(); + point_cloud->header.stamp = static_cast(timestamp * 1e6); + point_cloud->header.frame_id = frame_id; + point_cloud->height = 1; + for (uint16_t i = 0; i < count_num; i++) + { + double degree = 360.0 - points[i].degree; + bool pass_point = false; + if (angle_able_max < 360) + { + if (degree < angle_able_min || degree > angle_able_max) + pass_point = true; + } + else + { + if (degree < angle_able_min && degree > (angle_able_max - 360)) + pass_point = true; + } + if (points[i].range < 0.001) + pass_point = true; + if (!pass_point) + { + // printf("degree = %f\n",degree); + // printf("angle_able_min = %f\nangle_able_max=%f\n",angle_able_min,angle_able_max); + VPoint point; + int point_idx = round(degree * count_num / 360); + point.timestamp = timestamp - point_idx * (scan_time / count_num); + // printf("timestamp = %f\n",point.timestamp); + point.x = points[i].range * cos(M_PI / 180 * points[i].degree); + point.y = -points[i].range * sin(M_PI / 180 * points[i].degree); + point.z = 0; + point.intensity = points[i].intensity; + point_cloud->points.push_back(point); + ++point_cloud->width; + } + } + sensor_msgs::msg::PointCloud2 pc_msg; + pcl::toROSMsg(*point_cloud, pc_msg); + point_cloud_pub->publish(pc_msg); + } + } + count_num = 0; + wait_for_wake = true; + if (first_compensation && compensation) + { + lidar_difop(); + } + } + } + + bool LslidarDriver::polling() + { + if (!is_start) + return true; + // Allocate a new shared pointer for zero-copy sharing with other nodelets. + unsigned char *packet_bytes = new unsigned char[500]; + int len = 0; + bool difop = false; + if (interface_selection == "net") + { + auto packet = lslidar_msgs::msg::LslidarPacket::UniquePtr( + new lslidar_msgs::msg::LslidarPacket()); + + std_msgs::msg::Byte msg; + while (true) + { + difop = false; + len = 0; + // keep reading until full packet received + len = msop_input_->getPacket(packet); + if (packet->data[0] == 0x5a) + { + if (lidar_name == "N10" || lidar_name == "L10") + len = 58; + else if (lidar_name == "M10") + len = 92; + else if (lidar_name == "N10_P") + len = 108; + else if (lidar_name == "M10_GPS") + len = 102; + else + { + int len_H = packet->data[1]; + int len_L = packet->data[2]; + len = len_H * 256 + len_L; + } + for (int i = len - 1; i > 0; i--) + packet->data[i] = packet->data[i - 1]; + packet->data[0] = 0xa5; + } + + if (lidar_name == "N10" || lidar_name == "L10") + len = 58; + else if (lidar_name == "M10") + len = 92; + else if (lidar_name == "N10_P") + len = 108; + else if (lidar_name == "M10_GPS") + len = 102; + else + { + int len_H = packet->data[2]; + int len_L = packet->data[3]; + len = len_H * 256 + len_L; + } + if ((lidar_name == "M10" || lidar_name == "M10_DOUBLE" || lidar_name == "M10_GPS" || lidar_name == "M10_P" || lidar_name == "M10_PLUS") && compensation) + { + if (packet->data[2] == 0x55 && packet->data[3] == 0x00 && packet->data[186] == 0xFA && packet->data[187] == 0xFB) + { + len = 188; + difop = true; + } + } + + if (len <= 0 || len >= 1000 || packet->data[0] != 0xa5 || packet->data[1] != 0x5a) + continue; + for (int i = 0; i < len; i++) + { + packet_bytes[i] = packet->data[i]; + } + if ((lidar_name == "N10" || lidar_name == "L10" || lidar_name == "N10_P") && packet_bytes[len - 1] != N10_CalCRC8(packet_bytes, len - 1)) + continue; + break; + } + } + else + { + if (in_file_name != "") // 读txt文件功能 + { + int usleep_time = round(1000000 / 10 / 24) - 135; + while (true) + { + std::ifstream file_reader(in_file_name); + while (file_reader.peek() != EOF) + { + std::string line; + std::getline(file_reader, line, '\n'); + for (long unsigned int i = 0; i < line.size() - 1; i++) + { + line[i] = line[i] - 48; + if (line[i] > 9) + line[i] = line[i] - 39; + } + + for (long unsigned int i = 0; i < (line.size() - 1) / 2; i++) + { + packet_bytes[i] = line[i * 2] * 16 + line[i * 2 + 1]; + } + if (lidar_name == "N10" || lidar_name == "L10") + len = 58; + else if (lidar_name == "M10") + len = 92; + else if (lidar_name == "N10_P") + len = 108; + else if (lidar_name == "M10_GPS") + len = 102; + else + { + int len_H = packet_bytes[2]; + int len_L = packet_bytes[3]; + len = len_H * 256 + len_L; + } + if (lidar_name == "N10_P" || lidar_name == "M10_DOUBLE") + LslidarDriver::data_processing_2(packet_bytes, len); + else + LslidarDriver::data_processing(packet_bytes, len); + usleep(usleep_time); + } + } + return false; + } + else + { + while (true) + { + difop = false; + len = 0; + len = LslidarDriver::receive_data(packet_bytes); + if ((lidar_name == "M10" || lidar_name == "M10_DOUBLE" || lidar_name == "M10_GPS" || lidar_name == "M10_P" || lidar_name == "M10_PLUS") && compensation) + { + if (packet_bytes[2] == 0x55 && packet_bytes[3] == 0x00 && packet_bytes[186] == 0xFA && packet_bytes[187] == 0xFB) + difop = true; + } + if (len == 0) + continue; + break; + } + } + } + if (difop) + LslidarDriver::difop_processing(packet_bytes); + else + { + if (lidar_name == "N10_P" || lidar_name == "M10_DOUBLE") + LslidarDriver::data_processing_2(packet_bytes, len); + else + LslidarDriver::data_processing(packet_bytes, len); + } + delete packet_bytes; + return true; + } + +} // namespace lslidar_driver diff --git a/Lslidar_ROS2_driver/lslidar_driver/src/lslidar_driver_node.cc b/Lslidar_ROS2_driver/lslidar_driver/src/lslidar_driver_node.cc new file mode 100644 index 0000000..6a89f99 --- /dev/null +++ b/Lslidar_ROS2_driver/lslidar_driver/src/lslidar_driver_node.cc @@ -0,0 +1,35 @@ +/* + * This file is part of lslidar driver. + * + * The driver is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * The driver is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with the driver. If not, see . + */ + +#include "rclcpp/rclcpp.hpp" +#include "lslidar_driver/lslidar_driver.h" + +using namespace lslidar_driver; +volatile sig_atomic_t flag = 1; + +int main(int argc, char* argv[]) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + + while (rclcpp::ok() && node->polling()) { + rclcpp::spin_some(node); + } + //rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/Lslidar_ROS2_driver/lslidar_msgs/CMakeLists.txt b/Lslidar_ROS2_driver/lslidar_msgs/CMakeLists.txt new file mode 100644 index 0000000..e6a603e --- /dev/null +++ b/Lslidar_ROS2_driver/lslidar_msgs/CMakeLists.txt @@ -0,0 +1,39 @@ +cmake_minimum_required(VERSION 3.5) +project(lslidar_msgs) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +rosidl_generate_interfaces(lslidar_msgs + "msg/LslidarDifop.msg" + "msg/LslidarPacket.msg" + "msg/LslidarPoint.msg" + "msg/LslidarScan.msg" + "msg/LslidarSweep.msg" + DEPENDENCIES builtin_interfaces std_msgs + ) + +ament_package() diff --git a/Lslidar_ROS2_driver/lslidar_msgs/msg/LslidarDifop.msg b/Lslidar_ROS2_driver/lslidar_msgs/msg/LslidarDifop.msg new file mode 100644 index 0000000..f377c75 --- /dev/null +++ b/Lslidar_ROS2_driver/lslidar_msgs/msg/LslidarDifop.msg @@ -0,0 +1,2 @@ +int64 temperature +int64 rpm diff --git a/Lslidar_ROS2_driver/lslidar_msgs/msg/LslidarPacket.msg b/Lslidar_ROS2_driver/lslidar_msgs/msg/LslidarPacket.msg new file mode 100644 index 0000000..d77ee45 --- /dev/null +++ b/Lslidar_ROS2_driver/lslidar_msgs/msg/LslidarPacket.msg @@ -0,0 +1,5 @@ +# Raw Leishen LIDAR packet. + +builtin_interfaces/Time stamp # packet timestamp +uint8[2000] data # packet contents + diff --git a/Lslidar_ROS2_driver/lslidar_msgs/msg/LslidarPoint.msg b/Lslidar_ROS2_driver/lslidar_msgs/msg/LslidarPoint.msg new file mode 100644 index 0000000..3132167 --- /dev/null +++ b/Lslidar_ROS2_driver/lslidar_msgs/msg/LslidarPoint.msg @@ -0,0 +1,12 @@ +# Time when the point is captured +float32 time + +# Converted distance in the sensor frame +float64 x +float64 y +float64 z + +# Raw measurement from Leishen M10 +float64 azimuth +float64 distance +float64 intensity diff --git a/Lslidar_ROS2_driver/lslidar_msgs/msg/LslidarScan.msg b/Lslidar_ROS2_driver/lslidar_msgs/msg/LslidarScan.msg new file mode 100644 index 0000000..0a3891c --- /dev/null +++ b/Lslidar_ROS2_driver/lslidar_msgs/msg/LslidarScan.msg @@ -0,0 +1,6 @@ +# Altitude of all the points within this scan +float64 altitude + +# The valid points in this scan sorted by azimuth +# from 0 to 359.99 +LslidarPoint[] points diff --git a/Lslidar_ROS2_driver/lslidar_msgs/msg/LslidarSweep.msg b/Lslidar_ROS2_driver/lslidar_msgs/msg/LslidarSweep.msg new file mode 100644 index 0000000..9cfe4b7 --- /dev/null +++ b/Lslidar_ROS2_driver/lslidar_msgs/msg/LslidarSweep.msg @@ -0,0 +1,4 @@ +std_msgs/Header header + +# The 0th scan is at the bottom +LslidarScan[16] scans diff --git a/Lslidar_ROS2_driver/lslidar_msgs/package.xml b/Lslidar_ROS2_driver/lslidar_msgs/package.xml new file mode 100644 index 0000000..8001ad5 --- /dev/null +++ b/Lslidar_ROS2_driver/lslidar_msgs/package.xml @@ -0,0 +1,25 @@ + + + lslidar_msgs + 1.2.0 + ROS message definitions for Leishen LIDARs. + Nick Shu + Nick Shu + GNU General Public License V3.0 + + ament_cmake + rosidl_default_generators + + rosidl_default_runtime + builtin_interfaces + + std_msgs + ament_lint_auto + ament_lint_common + + rosidl_interface_packages + + + ament_cmake + + diff --git a/Lslidar_ROS2_driver/version.txt b/Lslidar_ROS2_driver/version.txt new file mode 100644 index 0000000..d00292d --- /dev/null +++ b/Lslidar_ROS2_driver/version.txt @@ -0,0 +1,40 @@ +版本变更 +/*************************************************************** +初始版本: LSLIDAR_M10_N10_V2.5.0_221104_ROS2 +变更内容: + 1.实现M10/M10_P/M10_PLUS/N10/M10_GPS网口和串口传输数据生成点云功能 + 2.实现点云角度裁剪和距离过滤功能 + 3.可以通过lslidar_order话题控制雷达启停 + 4.支持读取pcap包 + +更改日期: 2022-11-04 +***************************************************************/ + +/*************************************************************** +初始版本: LSLIDAR_M10_N10_V2.5.0_221104_ROS2 +变更版本：LSLIDAR_M10_N10_V2.5.1_221130_ROS2 +变更内容: + 1.新增L10雷达。 + 2.针对M10_P雷达出现的点云问题进行紧急修复。 + 3.针对M10和M10_GPS雷达在最近出货雷达出现的点云问题进行紧急修复。 + 4.修复循环索取新内存导致机器卡死bug。 + 5.更新M10_P和M10_PLUS雷达协议。 + 6.修复角度值分配错误问题。 + +更改日期: 2022-11-30 +***************************************************************/ + +/*************************************************************** +初始版本: LSLIDAR_M10_N10_V2.5.1_221130_ROS2 +变更版本：LSLIDAR_M10_N10_V2.5.2_230110_ROS2 +变更内容: + 1.修复驱动串口读取时包头检测漏洞。 + 2.更新M10系列的角度补偿方法。 + 3.修复发现的点角度不连续bug。 + 4.添加可选择是否发布pointcloud2话题。 + 5.添加可选择是否发布scan话题。 + 6.兼容ROS2的humble版本 + 7.兼容双回波雷达 + 8.添加txt文件读取方法 +更改日期: 2023-01-10 +***************************************************************/