add Lslidar_ROS2_driver/

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 
+
+
+

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()

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 <http://www.gnu.org/licenses/>.
+ *
+ *     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 <unistd.h>
+#include <stdio.h>
+#include <pcap.h>
+#include <netinet/in.h>
+#include "rclcpp/rclcpp.hpp"
+#include <lslidar_msgs/msg/lslidar_packet.hpp>
+#include <string>
+#include <sstream>
+#include <sys/socket.h>
+#include <arpa/inet.h>
+#include <poll.h>
+#include <errno.h>
+#include <fcntl.h>
+#include <sys/file.h>
+#include <signal.h>
+#include <sensor_msgs/msg/time_reference.hpp>
+#include <std_msgs/msg/int8.hpp>
+#include <cmath>
+
+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

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 <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <termios.h>
+#include <errno.h>
+#include <unistd.h>
+#include <string.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <stdint.h>
+#include <fstream>
+#include <iostream>
+
+//波特率
+#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
+

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 <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef LSLIDAR_DRIVER_H
+#define LSLIDAR_DRIVER_H
+
+#include <unistd.h>
+#include <stdio.h>
+#include <netinet/in.h>
+#include <string>
+
+#include <boost/shared_ptr.hpp>
+#include <boost/date_time/posix_time/posix_time.hpp>
+#include <boost/thread.hpp>
+#include "rclcpp/rclcpp.hpp"
+#include <thread>
+#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<LslidarDriver> LslidarDriverPtr;
+    typedef std::shared_ptr<const LslidarDriver> 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<ScanPoint> &points, rclcpp::Time &scan_time, float &scan_duration);
+
+    boost::thread *pubscan_thread_ ;
+    boost::shared_ptr<Input> 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<ScanPoint> scan_points_;
+    std::vector<ScanPoint> scan_points_bak_;
+    // Diagnostics updater
+    diagnostic_updater::Updater diagnostics;
+    std::shared_ptr<diagnostic_updater::TopicDiagnostic> diag_topic;
+    double diag_min_freq;
+    double diag_max_freq;
+    rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr scan_pub;
+    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr point_cloud_pub;
+	rclcpp::Subscription<std_msgs::msg::Int8>::SharedPtr difop_switch;
+    LSIOSR * serial_;
+};
+typedef PointXYZIT VPoint;
+typedef pcl::PointCloud<VPoint> 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_

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,
+    ])
+

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,
+    ])
+

Lslidar_ROS2_driver/lslidar_driver/package.xml

@@ -0,0 +1,36 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>lslidar_driver</name>
+  <version>1.2.0</version>
+  <description>ROS device driver for Leishen lidar.</description>
+  <maintainer email="shaohuashu@lslidar.com">Nick Shu</maintainer>
+  <author>Nick Shu</author>
+  <license>GNU General Public License V3.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>rclcpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>lslidar_msgs</build_depend>
+  <build_depend>pcl_conversions</build_depend>
+  <build_depend>rclpy</build_depend>
+  <build_depend>libpcap</build_depend>
+  <build_depend>libpcl-all-dev</build_depend>
+  <build_depend>pluginlib</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+
+  <exec_depend>rclcpp</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>lslidar_msgs</exec_depend>
+  <exec_depend>pcl_conversions</exec_depend>
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>libpcap</exec_depend>
+  <exec_depend>libpcl-all</exec_depend>
+  <exec_depend>pluginlib</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+
+  <depend>diagnostic_updater</depend>
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

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文件读取功能

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文件读取功能

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文件读取功能

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: <Fixed 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: <Fixed 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

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<std::string>("device_ip","192.168.1.102");
+        private_nh->declare_parameter<std::string>("device_ip_difop","192.168.1.200");
+        private_nh->declare_parameter<bool>("add_multicast",false);
+        private_nh->declare_parameter<std::string>("group_ip","224.1.1.2");
+        private_nh->declare_parameter<int>("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

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;
+}
+
+}

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 <http://www.gnu.org/licenses/>.
+ */
+
+#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<lslidar_driver::LslidarDriver>();
+  
+  while (rclcpp::ok() && node->polling()) {
+        rclcpp::spin_some(node);
+  }
+  //rclcpp::spin(node);
+  rclcpp::shutdown();
+  return 0;
+}

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()

Lslidar_ROS2_driver/lslidar_msgs/msg/LslidarDifop.msg

@@ -0,0 +1,2 @@
+int64 temperature
+int64 rpm

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
+

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

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

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

Lslidar_ROS2_driver/lslidar_msgs/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>lslidar_msgs</name>
+  <version>1.2.0</version>
+  <description>ROS message definitions for Leishen LIDARs.</description>
+  <maintainer email="shaohuashu@lslidar.com">Nick Shu</maintainer>
+  <author>Nick Shu</author>
+  <license>GNU General Public License V3.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <exec_depend>builtin_interfaces</exec_depend>
+
+  <depend>std_msgs</depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+  
+  <member_of_group>rosidl_interface_packages</member_of_group>
+  
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

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
+***************************************************************/