houyongdi/dreamDeckAutopilot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Initial commit

Browse Source
This commit is contained in:
houyongdi
2026-07-27 13:51:19 +08:00
commit 7bec56ca51
5408 changed files with 1126933 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sensors/lslidar_m10_driver/README.md
+188
View File
@@ -0,0 +1,188 @@
## Lslidar_M10_N10_V2.5.3_231110_ROS使用说明
## 1.工程介绍
​ Lslidar_M10_N10_V2.5.3_231110_ROS为linux环境下雷达ros驱动，适用于M10 ,M10_GPS,M10_P,M10_PLUS,N10,N10_PLUS 和 L10.版本雷达，程序在ubuntu 20.04 ros noetic，ubuntu18.04 ros melodic以及ubuntu16.04 ros kinetic下测试通过。
## 2.依赖
- ubuntu20.04 ros noetic/ubuntu18.04 ros melodic/ubuntu16.04 ros kinetic
- ros依赖
```bash
# 安装
sudo apt-get install ros-$ROS_DISTRO-pcl-ros ros-$ROS_DISTRO-pluginlib ros-$ROS_DISTRO-pcl-conversions ros-$ROS_DISTRO-diagnostic-updater
```
- 其他依赖
~~~bash
sudo apt-get install libpcap-dev
~~~
## 3.编译运行
### 3.1 编译
~~~bash
mkdir -p ~/lidar_ws/src #lidar_ws支持自定义命名(禁止空格和中文)，src目录名不能改变
#将驱动压缩包解压缩放到~/lidar_ws/src 目录下
cd ~/lidar_ws
catkin_make
source devel/setup.bash
~~~
### 3.2 运行
运行单个雷达:
~~~bash
roslaunch lslidar_driver lslidar_net.launch #网口版雷达
roslaunch lslidar_driver lslidar_serial.launch #串口版雷达
~~~
运行多个雷达：
~~~bash
roslaunch lslidar_driver lslidar_net_double.launch #两台网口版雷达
roslaunch lslidar_driver lslidar_serial_double.launch #两台串口版雷达
roslaunch lslidar_driver lslidar_net_four.launch #四台网口版雷达
roslaunch lslidar_driver lslidar_serial_four.launch #四台串口版雷达
~~~
## 4.参数介绍
#### lslidar_serial.launch 文件内容如下，每个参数含义见注释说明。
~~~xml
<launch>
<node pkg="lslidar_driver" type="lslidar_driver_node" name="lslidar_driver_node" output="screen">
<param name="lidar_name" value="M10"/> #雷达选择:M10 M10_P M10_PLUS M10_GPS N10 N10_PLUS L10
<param name="serial_port" value="/dev/ttyUSB0"/> #雷达连接的串口
<param name="interface_selection" value="serial"/> #接口选择:net 为网口,serial 为串口
<param name="frame_id" value="laser_link"/> #激光坐标
<param name="scan_topic" value="scan"/> #设置激光数据topic名称
<param name="angle_disable_min" value="0.0"/> #角度裁剪开始值
<param name="angle_disable_max" value="0.0"/> #角度裁剪结束值
<param name="min_range" value="0.0"/> #雷达接收距离最小值
<param name="max_range" value="100.0"/> #雷达接收距离最大值
<param name="use_gps_ts" value="false"/> #雷达是否使用GPS授时
<param name="compensation" value="false"/> #雷达是否使用角度补偿功能
<param name="pubScan" value="true"/> #雷达是否发布scan话题
<param name="pubPointCloud2" value="false"/> #雷达是否发布pointcloud2话题
<param name="high_reflection" value="false"/> #M10_P雷达需填写该值,若不确定，请联系技术支持。
<!--param name="in_file_name" value="$(find lslidar_driver)/pcap/xxx.txt"/--> #驱动是否使用txt文件读取功能
</node>
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find lslidar_driver)/rviz/lslidar.rviz" output="screen"/> #雷达点云配置
</launch>
~~~
#### lslidar_net.launch 文件内容如下，每个参数含义见注释说明。
~~~xml
<launch>
<node pkg="lslidar_driver" type="lslidar_driver_node" name="lslidar_driver_node" output="screen">
<param name="frame_id" value="laser_link"/> #激光坐标
<param name="device_ip" value="192.168.1.200"/> #雷达源IP
<param name="device_port" value="2368"/> #雷达目的端口号
<param name="device_ip_difop" value="192.168.1.102"/>#雷达目的ip
<param name="difop_port" value="2369"/> #雷达源端口号
<param name="lidar_name" value="M10"/> #雷达选择:M10 M10_P M10_PLUS M10_GPS N10 N10_PLUS L10
<param name="interface_selection" value="net"/> #接口选择:net 为网口,serial 为串口
<param name="add_multicast" value="false"/> #是否启动组播。
<param name="group_ip" value="224.1.1.2"/> #组播ip。
<param name="scan_topic" value="scan"/> #设置激光数据topic名称
<param name="angle_disable_min" value="0.0"/> #角度裁剪开始值
<param name="angle_disable_max" value="0.0"/> #角度裁剪结束值
<param name="min_range" value="0"/> #雷达接收距离最小值
<param name="max_range" value="100.0"/> #雷达接收距离最大值
<param name="use_gps_ts" value="false"/> #雷达是否使用GPS授时
<param name="compensation" value="false"/> #雷达是否使用角度补偿功能
<param name="pubScan" value="true"/> #雷达是否发布scan话题
<param name="pubPointCloud2" value="false"/> #雷达是否发布pointcloud2话题
<param name="high_reflection" value="false"/> #M10_P雷达需填写该值,若不确定，请联系技术支持。
<!--param name="pcap" value="$(find lslidar_driver)/pcap/xxx.pcap"/--> #雷达是否使用pcap包读取功能
</node>
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find lslidar_driver)/rviz/lslidar.rviz" output="screen"/> #雷达点云配置
</launch>
~~~
## 5.命令控制雷达
#### 运行驱动后，新开一个端口
~~~bash
#M10 M10_GPS M10_PLUS N10 N10_PLUS 雷达
rostopic pub -1 /lslidar_order std_msgs/Int8 "data: 0" #雷达停止旋转
rostopic pub -1 /lslidar_order std_msgs/Int8 "data: 1" #雷达开始旋转
#M10 M10_GPS M10_PLUS 雷达
rostopic pub -1 /lslidar_order std_msgs/Int8 "data: 2" #雷达点云不滤波
rostopic pub -1 /lslidar_order std_msgs/Int8 "data: 3" #雷达点云正常滤波
rostopic pub -1 /lslidar_order std_msgs/Int8 "data: 4" #雷达近距离滤波
rostopic pub -1 /lslidar_order std_msgs/Int8 "data: 100" #雷达发设备包
#M10启停雷达
rostopic pub -1 /lslidar_order std_msgs/Int8 "data: 30" #雷达停转并停止发数据
~~~
### 组播模式：
- 上位机设置雷达开启组播模式
- 修改launch文件以下参数
~~~shell
<param name="add_multicast" value="false"/> #是否添加组播
<param name="group_ip" value="224.1.1.2"/> #组播的ip
~~~
- 运行以下指令将电脑加入组内（将指令中的enp2s0替换为用户电脑的网卡名,可用ifconfig查看网卡名)
~~~shell
ifconfig
sudo route add -net 224.0.0.0/4 dev enp2s0
~~~
### 离线模式：
#### 播pcap包
- 把录制好的pcap文件，拷贝到lslidar_driver/pcap文件夹下。
- 修改launch文件以下参数
~~~shell
#取消注释
<param name="pcap" value="$(find lslidar_cx_driver)/pcap/xxx.pcap" /> #pcap包路径，加载pcap包时打开此注释
~~~
#### 播txt文件
- 把录制好的txt文件 路径写入launch文件。
~~~shell
#取消注释
<!--param name="in_file_name" value="$(find lslidar_driver)/pcap/xxx.txt"/--> #txt文件路径，加载txt文件时打开此注释
~~~
sensors/lslidar_m10_driver/README_en.md
+174
View File
@@ -0,0 +1,174 @@
# Lslidar_M10_N10_V2.5.3_231110_ROS使用说明
## 1.Introduction
​ Lslidar_M10_N10_V2.5.3_231110_ROS is the lidar ros driver in linux environment, which is suitable for M10 ,M10_GPS,M10_P,M10_PLUS,N10,N10_PLUS and L10 lidar. The program has tested under ubuntu 20.04 ros noetic , ubuntu18.04 ros melodic and ubuntu16.04 ros kinetic.
## 2.Dependencies
- #### ros
​ To run lidar driver in ROS environment, ROS related libraries need to be installed.
​ **Ubuntu 16.04**: ros-kinetic-desktop-full
​ **Ubuntu 18.04**: ros-melodic-desktop-full
​ **Ubuntu 20.04**: ros-noetic-desktop-full
​ **Installation**: please refer to [http://wiki.ros.org]
- #### ros dependencies
```bash
# install
sudo apt-get install ros-$ROS_DISTRO-pcl-ros ros-$ROS_DISTRO-pluginlib ros-$ROS_DISTRO-pcl-conversions ros-$ROS_DISTRO-diagnostic-updater
```
- #### other dependencies
~~~bash
sudo apt-get install libpcap-dev
sudo apt-get install libboost${BOOST_VERSION}-dev #Select the appropriate version
~~~
## 3.Compile && Run
- 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.
```bash
mkdir -p ~/lidar_ws/src
#Copy the whole lidar ROS driver directory into ROS workspace, i.e "~/lidar_ws/src".
cd ~/lidar_ws
catkin_make
source devel/setup.bash
#run
roslaunch lslidar_driver lslidar_net.launch #net lidar
roslaunch lslidar_driver lslidar_serial.launch #serial lidar
```
## 4.Introduction to parameters
- The content of the lslidar_serial.launch file is as follows, and the meaning of each parameter is shown in the notes.
#### When using serial port radar, serial port permissions should be granted
~~~bash
sudo chmod 777 /dev/ttyUSB0 #ttyUSB0 is lidar serial port
~~~
~~~xml
<launch>
<node pkg="lslidar_driver" type="lslidar_driver_node" name="lslidar_driver_node" output="screen">
<param name="lidar_name" value="M10"/> #lidar type:M10 M10_P M10_PLUS M10_GPS N10 N10_PLUS L10
<param name="serial_port" value="/dev/ttyUSB0"/> #lidar connection serial port
<param name="interface_selection" value="serial"/> #interface select: net or serial
<param name="frame_id" value="laser_link"/> #lidar coordinates
<param name="scan_topic" value="scan"/> #Topic name of lidar scan
<param name="angle_disable_min" value="0.0"/> #Crop angle start
<param name="angle_disable_max" value="0.0"/> #Crop angle end
<param name="min_range" value="0.0"/> #lidar receiving distance min
<param name="max_range" value="100.0"/> #lidar receiving distance max
<param name="use_gps_ts" value="false"/> #lidar use GPS timing
<param name="compensation" value="false"/> #Using angle compensation
<param name="pubScan" value="true"/> #Lidar Release Scanning Theme
<param name="pubPointCloud2" value="false"/> #Lidar Release pointcloud2 Theme
<param name="high_reflection" value="false"/> #M10_P lidar This value needs to be filled in,Uncertain, please contact technical support.
<!--param name="in_file_name" value="$(find lslidar_driver)/pcap/xxx.txt"/--> #Using the txt file reading function
</node>
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find lslidar_driver)/rviz/lslidar.rviz" output="screen"/> #Visualization
</launch>
~~~
- The content of the lslidar_net.launch file is as follows, and the meaning of each parameter is shown in the notes.
~~~xml
<launch>
<node pkg="lslidar_driver" type="lslidar_driver_node" name="lslidar_driver_node" output="screen">
<param name="frame_id" value="laser_link"/> #lidar scan/point cloud coordinate system name
<param name="device_ip" value="192.168.1.200"/> #lidar ip
<param name="device_port" value="2368"/> #Main data Stream Output Protocol packet port
<param name="device_ip_difop" value="192.168.1.102"/>#lidar destination IP
<param name="difop_port" value="2369"/> #Device Information Output Protocol packet port
<param name="lidar_name" value="M10"/> #lidar type:M10 M10_P M10_PLUS M10_GPS N10 N10_PLUS L10M10_GPS N10 N10_PLUS L10
<param name="interface_selection" value="net"/> #interface select: net or serial
<param name="add_multicast" value="false"/> #Whether to add multicast
<param name="group_ip" value="224.1.1.2"/> #multicast ip
<param name="scan_topic" value="scan"/> #设置激光数据topic名称
<param name="angle_disable_min" value="0.0"/> #Crop angle start
<param name="angle_disable_max" value="0.0"/> #Crop angle end
<param name="min_range" value="0"/> #lidar receiving distance min
<param name="max_range" value="100.0"/> #lidar receiving distance max
<param name="use_gps_ts" value="false"/> #lidar use GPS timing
<param name="compensation" value="false"/> #Using angle compensation
<param name="pubScan" value="true"/> #Lidar Release Scanning Theme
<param name="pubPointCloud2" value="false"/> #Lidar Release pointcloud2 Theme
<param name="high_reflection" value="false"/> #M10_P lidar This value needs to be filled in,Uncertain, please contact technical support.
<!--param name="pcap" value="$(find lslidar_driver)/pcap/xxx.pcap"/--> #Using the pcap file reading function
</node>
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find lslidar_driver)/rviz/lslidar.rviz" output="screen"/> #Visualization
</launch>
~~~
## 5.Command controlled radar
- Run lidar drive,open new terminal
~~~bash
#lidar type: M10 M10_GPS M10_PLUS N10 N10_PLUS
rostopic pub -1 /lslidar_order std_msgs/Int8 "data: 0" #close radar
rostopic pub -1 /lslidar_order std_msgs/Int8 "data: 1" #open radar
#lidar type: M10 M10_GPS M10_P
rostopic pub -1 /lslidar_order std_msgs/Int8 "data: 2" #no filtering
rostopic pub -1 /lslidar_order std_msgs/Int8 "data: 3" #normal filtering
rostopic pub -1 /lslidar_order std_msgs/Int8 "data: 4" #close range filtering
rostopic pub -1 /lslidar_order std_msgs/Int8 "data: 100" #Lidar sends device packets
#Only M10 Start stop version
rostopic pub -1 /lslidar_order std_msgs/Int8 "data: 30" #close radar & stop send data
~~~
#### Offline pcap mode:
- Modify the following parameters of the launch file
~~~bash
#uncomment
<param name="pcap" value="$(find lslidar_driver)/pcap/xxx.pcap"/>
#Uncomment to read the data from the pcap file, and add the comment to read the data from the lidar
~~~
#### Offline txt mode:
- Modify the following parameters of the launch file
~~~bash
#uncomment
<param name="in_file_name" value="$(find lslidar_driver)/pcap/xxx.txt"/>
#Uncomment to read the data from the pcap file, and add the comment to read the data from the lidar
~~~
sensors/lslidar_m10_driver/lslidar/CMakeLists.txt
+4
View File
@@ -0,0 +1,4 @@
cmake_minimum_required(VERSION 2.8.3)
project(lslidar)
find_package(catkin REQUIRED)
catkin_metapackage()
sensors/lslidar_m10_driver/lslidar/package.xml
+22
View File
@@ -0,0 +1,22 @@
<package>
<name>lslidar</name>
<version>1.2.0</version>
<description>
Basic ROS support for the Leishen M10 N10 and L10 LIDARs.
</description>
<maintainer email="honghangli@lslidar.com">Nick Li</maintainer>
<author>Nick Li</author>
<license>GNU General Public License V3.0</license>
<buildtool_depend>catkin</buildtool_depend>
<run_depend>lslidar_driver</run_depend>
<run_depend>lslidar_msgs</run_depend>
<export>
<metapackage/>
</export>
</package>
sensors/lslidar_m10_driver/lslidar_driver/CMakeLists.txt
+110
View File
@@ -0,0 +1,110 @@
cmake_minimum_required(VERSION 2.8.3)
project(lslidar_driver)
add_compile_options(-std=c++14)
set( CMAKE_CXX_STANDARD 14)
set(libpcap_LIBRARIES -lpcap)
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
sensor_msgs
diagnostic_updater
nodelet
pcl_ros
pcl_conversions
lslidar_msgs
)
find_package(Boost REQUIRED)
catkin_package(
INCLUDE_DIRS include
# LIBRARIES lslidar_driver
CATKIN_DEPENDS
roscpp diagnostic_updater nodelet
pcl_ros pcl_conversions
lslidar_msgs
DEPENDS
boost
)
include_directories(
include
${Boost_INCLUDE_DIR}
${catkin_INCLUDE_DIRS}
)
link_directories(
${catkin_LIBRARY_DIRS}
)
add_library(lslidar_input
src/input.cc
)
add_library(lslidar_serial
src/lsiosr.cpp
)
# Leishen lidar driver
add_library(lslidar_driver
src/lslidar_driver.cc
)
target_link_libraries(lslidar_input
${catkin_LIBRARIES}
${libpcap_LIBRARIES}
)
add_dependencies(lslidar_input
lslidar_msgs_generate_messages_cpp
)
target_link_libraries(lslidar_serial
${catkin_LIBRARIES}
)
target_link_libraries(lslidar_driver
lslidar_input
lslidar_serial
${catkin_LIBRARIES}
)
add_dependencies(lslidar_driver
lslidar_input
lslidar_serial
${${PROJECT_NAME}_EXPORTED_TARGETS}
${catkin_EXPORTED_TARGETS}
)
# Leishen lidar nodelet
add_executable(lslidar_driver_node
src/lslidar_driver_node.cc
)
target_link_libraries(lslidar_driver_node
lslidar_driver
lslidar_input
lslidar_serial
${catkin_LIBRARIES}
)
add_dependencies(lslidar_driver_node
lslidar_input
lslidar_serial
${${PROJECT_NAME}_EXPORTED_TARGETS}
${catkin_EXPORTED_TARGETS}
)
# install options
install(TARGETS lslidar_input lslidar_driver_node lslidar_serial
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
install(DIRECTORY launch
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
sensors/lslidar_m10_driver/lslidar_driver/include/lslidar_driver/input.h
+138
View File
@@ -0,0 +1,138 @@
/*
* 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 <ros/ros.h>
#include <lslidar_msgs/LslidarPacket.h>
#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/TimeReference.h>
#include <std_msgs/Int8.h>
namespace lslidar_driver
{
static uint16_t PACKET_SIZE_input = 1206;
static uint16_t MSOP_DATA_PORT_NUMBER = 2368; // lslidar default data port on PC
static uint16_t DIFOP_DATA_PORT_NUMBER = 2369; // lslidar default difop 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(ros::NodeHandle private_nh, uint16_t port);
virtual ~Input()
{
}
virtual int getPacket(lslidar_msgs::LslidarPacketPtr& packet) = 0;
int getRpm(void);
int getReturnMode(void);
bool getUpdateFlag(void);
void clearUpdateFlag(void);
void UDP_order(const std_msgs::Int8 msg);
void UDP_difop();
void UDP_M10();
protected:
ros::NodeHandle 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(ros::NodeHandle private_nh, uint16_t port = MSOP_DATA_PORT_NUMBER);
virtual ~InputSocket();
//virtual int getPacket(lslidar_ch_msgs::LslidarPacket* pkt, const double time_offset);
virtual int getPacket(lslidar_msgs::LslidarPacketPtr& packet);
private:
private:
in_addr devip_;
in_addr devip_difop;
//struct ip_mreq group;
};
class InputPCAP : public Input
{
public:
InputPCAP(ros::NodeHandle private_nh,uint16_t port = MSOP_DATA_PORT_NUMBER, double packet_rate = 0.0,
std::string filename="",bool read_once = false,bool read_fast = false,double repeat_delay = 0.0);
virtual ~InputPCAP();
virtual int getPacket(lslidar_msgs::LslidarPacketPtr& pkt);
private:
ros::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
sensors/lslidar_m10_driver/lslidar_driver/include/lslidar_driver/lsiosr.h
+88
View File
@@ -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
sensors/lslidar_m10_driver/lslidar_driver/include/lslidar_driver/lslidar_driver.h
+165
View File
@@ -0,0 +1,165 @@
/*
* 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 "input.h"
#include <boost/shared_ptr.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/thread.hpp>
#include <ros/ros.h>
#include <diagnostic_updater/diagnostic_updater.h>
#include <diagnostic_updater/publisher.h>
#include "lsiosr.h"
#include <sensor_msgs/LaserScan.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <lslidar_msgs/LslidarPacket.h>
#include <std_msgs/Byte.h>
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;
uint16_t PACKET_SIZE ;
class LslidarDriver {
public:
LslidarDriver(ros::NodeHandle& n, ros::NodeHandle& pn);
~LslidarDriver();
bool initialize();
bool polling();
int getScan(std::vector<ScanPoint> &points, ros::Time &scan_time, double &scan_duration);
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);
typedef boost::shared_ptr<LslidarDriver> LslidarDriverPtr;
typedef boost::shared_ptr<const LslidarDriver> LslidarDriverConstPtr;
void recvThread_crc(int &count_2,int &link_time);
private:
boost::mutex mutex_;
boost::mutex pubscan_mutex_;
boost::condition_variable pubscan_cond_;
boost::thread *recv_thread_;
boost::thread *pubscan_thread_ ;
bool createRosIO();
void close_serial();
void open_serial();
void pubScanThread();
void lidar_difop();
void lidar_order(const std_msgs::Int8 msg);
int receive_data(unsigned char *packet_bytes);
void initParam();
uint8_t N10_CalCRC8(unsigned char * p, int len);
// Ethernet relate variables
int UDP_PORT_NUMBER;
bool is_start;
// ROS related variables
LSIOSR * serial_;
std::string serial_port_;
ros::NodeHandle nh;
ros::NodeHandle pnh;
std::string interface_selection;
boost::shared_ptr<Input> msop_input_;
ros::Publisher packet_pub;
ros::Publisher pointcloud_pub;
ros::Subscriber difop_switch;
// Diagnostics updater
diagnostic_updater::Updater diagnostics;
boost::shared_ptr<diagnostic_updater::TopicDiagnostic> diag_topic;
double diag_min_freq;
double diag_max_freq;
std::vector<ScanPoint> scan_points_;
std::vector<ScanPoint> scan_points_bak_;
std::string frame_id;
std::string lidar_name;
std::string scan_topic;
std::string dump_file;
std::string pointcloud_topic;
std::string in_file_name;
double min_range;
double max_range;
double angle_disable_min;
double angle_disable_max;
double angle_able_min;
double angle_able_max;
double degree_compensation = 0.0;
bool use_gps_ts;
bool high_reflection;
bool compensation;
bool first_compensation = true;
bool restart = true;
bool pubScan;
bool pubPointCloud2;
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_;
ros::Time pre_time_;
ros::Time time_;
ros::Publisher pub_;
tm pTime;
uint64_t sub_second;
uint64_t get_gps_stamp(tm t);
uint64_t sweep_end_time_gps;
uint64_t sweep_end_time_hardware;
int idx = 0;
int link_time = 0;
double last_degree = 0.0;
double packet_timestamp;
double last_packet_timestamp;
};
typedef LslidarDriver::LslidarDriverPtr LslidarDriverPtr;
typedef LslidarDriver::LslidarDriverConstPtr LslidarDriverConstPtr;
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_
sensors/lslidar_m10_driver/lslidar_driver/launch/lslidar_net.launch
+26
View File
@@ -0,0 +1,26 @@
<launch>
<node pkg="lslidar_driver" type="lslidar_driver_node" name="lslidar_driver_node" output="screen">
<param name="frame_id" value="laser_link"/> #激光坐标
<param name="device_ip" value="192.168.1.200"/> #雷达源IP
<param name="device_port" value="2368"/> #雷达目的端口号
<param name="device_ip_difop" value="192.168.1.102"/> #雷达目的ip
<param name="difop_port" value="2369"/> #雷达源端口号
<param name="lidar_name" value="M10"/> #雷达选择:M10 M10_P M10_PLUS M10_GPS N10 N10_PLUS L10
<param name="interface_selection" value="net"/> #接口选择:net 为网口,serial 为串口
<param name="add_multicast" value="false"/> #是否启动组播
<param name="group_ip" value="224.1.1.2"/> #组播ip
<param name="scan_topic" value="/scan"/> #设置激光数据topic名称
<param name="angle_disable_min" value="0.0"/> #角度裁剪开始值
<param name="angle_disable_max" value="0.0"/> #角度裁剪结束值
<param name="min_range" value="0"/> #雷达接收距离最小值
<param name="max_range" value="100.0"/> #雷达接收距离最大值
<param name="use_gps_ts" value="false"/> #雷达是否使用GPS授时
<param name="compensation" value="false"/> #雷达是否使用角度补偿功能
<param name="pubScan" value="true"/> #雷达是否发布scan话题
<param name="pubPointCloud2" value="false"/> #雷达是否发布pointcloud2话题
<param name="high_reflection" value="false"/> #M10_P雷达需填写该值,若不确定，请联系技术支持
<!--param name="pcap" value="$(find lslidar_driver)/pcap/xxx.pcap"/--> #雷达是否使用pcap包读取功能
</node>
</launch>
sensors/lslidar_m10_driver/lslidar_driver/package.xml
+35
View File
@@ -0,0 +1,35 @@
<package>
<name>lslidar_driver</name>
<version>1.2.0</version>
<description>
ROS device driver for Leishen M10 and N10 lidar.
</description>
<maintainer email="honghangli@lslidar.com">Nick Li</maintainer>
<author>Nick Li</author>
<license>GNU General Public License V3.0</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>diagnostic_updater</build_depend>
<build_depend>nodelet</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>lslidar_msgs</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>libpcl-all-dev</build_depend>
<build_depend>libpcl-all</build_depend>
<build_depend>rospy</build_depend>
<build_depend>pcl_ros</build_depend>
<build_depend>pcl_conversions</build_depend>
<run_depend>diagnostic_updater</run_depend>
<run_depend>nodelet</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>lslidar_msgs</run_depend>
<run_depend>sensor_msgs</run_depend>
<run_depend>libpcl-all-dev</run_depend>
<run_depend>libpcl-all</run_depend>
<run_depend>rospy</run_depend>
<run_depend>pcl_ros</run_depend>
<run_depend>pcl_conversions</run_depend>
</package>
sensors/lslidar_m10_driver/lslidar_driver/rviz/lslidar.rviz
+184
View File
@@ -0,0 +1,184 @@
Panels:
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
Splitter Ratio: 0.5
Tree Height: 434
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Name: Time
SyncMode: 0
SyncSource: LaserScan
Preferences:
PromptSaveOnExit: true
Toolbars:
toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/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/LaserScan
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 0
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Name: LaserScan
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Flat Squares
Topic: /scan
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
- Alpha: 1
Class: rviz/Axes
Enabled: false
Length: 1
Name: Axes
Radius: 0.10000000149011612
Reference Frame: <Fixed Frame>
Show Trail: false
Value: false
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Flat Squares
Topic: /lslidar_point_cloud
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Default Light: true
Fixed Frame: laser_link
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Theta std deviation: 0.2617993950843811
Topic: /initialpose
X std deviation: 0.5
Y std deviation: 0.5
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 2.459817886352539
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Field of View: 0.7853981852531433
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: 1.0497968196868896
Target Frame: <Fixed Frame>
Yaw: 3.180391788482666
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 731
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd00000004000000000000016a0000023dfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000023d000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000023dfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000023d000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000003bc00fffffffb0000000800540069006d006501000000000000045000000000000000000000022b0000023d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1200
X: 390
Y: 312
sensors/lslidar_m10_driver/lslidar_driver/src/input.cc
+446
View File
@@ -0,0 +1,446 @@
/*
* 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/>.
*/
#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(ros::NodeHandle private_nh, uint16_t port) : private_nh_(private_nh), port_(port)
{
npkt_update_flag_ = false;
cur_rpm_ = 0;
return_mode_ = 1;
private_nh.param("device_ip", devip_str_, std::string(""));
private_nh.param("lidar_name", lidar_name, std::string("M10"));
private_nh.param("device_ip_difop", devip_str_difop, std::string("192.168.1.102"));
private_nh.param<bool>("add_multicast", add_multicast, false);
private_nh.param<std::string>("group_ip", group_ip, "224.1.1.2");
private_nh.param<int>("difop_port", UDP_PORT_NUMBER_DIFOP, 2369);
if (!devip_str_.empty())
ROS_INFO_STREAM("Only accepting packets from IP address: " << devip_str_);
}
/** @brief constructor
*
* @param private_nh ROS private handle for calling node.
* @param port UDP port number
*/
InputSocket::InputSocket(ros::NodeHandle 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);
}
ROS_INFO_STREAM("Opening UDP socket: port " << 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 = htonl(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;
// char *group_ip_ = (char *) group_ip.c_str();
group.imr_multiaddr.s_addr = inet_addr(group_ip.c_str());
// group.imr_interface.s_addr = INADDR_ANY;
group.imr_interface.s_addr = htonl(INADDR_ANY);
// group.imr_interface.s_addr = inet_addr("192.168.1.102");
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());
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[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_M10()
{
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[184] = 0x01;
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) return;
}
return;
}
void Input::UDP_order(const std_msgs::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" || lidar_name == "M10_DOUBLE" ){
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 == 30){ //雷达停转并停止发数据
data[184] = 0x03;
data[185] = 0x00;
}
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
{
ROS_INFO("Successfully set!");
if (i == 1)
usleep(3000000);
return;
}
}
return;
}
int InputSocket::getPacket(lslidar_msgs::LslidarPacketPtr &packet)
{
double time1 = ros::Time::now().toSec();
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 (true)
while (flag == 1)
{
// poll() until input available
do
{
int retval = poll(fds, 1, POLL_TIMEOUT);
if (retval < 0) // poll() error?
{
if (errno != EINTR)
ROS_ERROR("poll() error: %s", strerror(errno));
return 0;
}
if (retval == 0) // poll() timeout?
{
ROS_WARN("lslidar poll() timeout");
return 0;
}
if ((fds[0].revents & POLLERR) || (fds[0].revents & POLLHUP) || (fds[0].revents & POLLNVAL)) // device error?
{
ROS_ERROR("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");
ROS_INFO("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();
}
// this->getFPGA_GPSTimeStamp(packet);
// Average the times at which we begin and end reading. Use that to
// estimate when the scan occurred.
// double time2 = ros::Time::now().toSec();
// packet->stamp = ros::Time((time2 + time1) / 2.0);
// packet->stamp = this->timeStamp;
return q;
}
InputPCAP::InputPCAP(ros::NodeHandle private_nh, uint16_t port, double packet_rate, std::string filename,
bool read_once, bool read_fast, double repeat_delay) : Input(private_nh, port),
packet_rate_(packet_rate),
filename_(filename)
{
pcap_ = NULL;
empty_ = true;
private_nh.param("read_once", read_once_, false);
private_nh.param("read_fast", read_fast_, false);
private_nh.param("repeat_delay", repeat_delay_, 0.0);
if (read_once_)
ROS_INFO("Read input file only once.");
if (read_fast_)
ROS_INFO("Read input file as quickly as possible.");
if (repeat_delay_ > 0.0)
ROS_INFO("Delay %.3f seconds before repeating input file.", repeat_delay_);
ROS_INFO_STREAM("Opening PCAP file " << filename_);
if ((pcap_ = pcap_open_offline(filename_.c_str(), errbuf_)) == NULL)
{
ROS_FATAL("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::LslidarPacketPtr &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);
pkt->stamp = ros::Time::now();
empty_ = false;
return 0;
}
if (empty_)
{
ROS_WARN("Error %d reading lslidar packet: %s", res, pcap_geterr(pcap_));
return -1;
}
if (read_once_)
{
ROS_INFO("end of file reached -- done reading.");
return -1;
}
if (repeat_delay_ > 0.0)
{
ROS_INFO("end of file reached -- delaying %.3f seconds.", repeat_delay_);
usleep(rint(repeat_delay_ * 1000000.0));
}
ROS_DEBUG("replayding lsliar dump file");
pcap_close(pcap_);
pcap_ = pcap_open_offline(filename_.c_str(), errbuf_);
empty_ = true;
}
if (flag == 0)
{
abort();
}
return 0;
}
} // namespace
sensors/lslidar_m10_driver/lslidar_driver/src/lsiosr.cpp
+399
View File
@@ -0,0 +1,399 @@
/*******************************************************
@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_);
}
std::string LSIOSR::getPort()
{
return port_;
}
int LSIOSR::setPortName(std::string name)
{
port_ = name;
return 0;
}
}
sensors/lslidar_m10_driver/lslidar_driver/src/lslidar_driver.cc
+1345
View File
File diff suppressed because it is too large Load Diff
sensors/lslidar_m10_driver/lslidar_driver/src/lslidar_driver_node.cc
+48
View File
@@ -0,0 +1,48 @@
/*
* 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 <ros/ros.h>
#include <lslidar_driver/lslidar_driver.h>
volatile sig_atomic_t flag = 1;
static void my_handler(int sig)
{
flag = 0;
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "lslidar_driver_node");
ros::NodeHandle node;
ros::NodeHandle private_nh("~");
// start the driver
lslidar_driver::LslidarDriver driver(node, private_nh);
if (!driver.initialize()) {
ROS_ERROR("Cannot initialize lslidar driver...");
return 0;
}
// loop until shut down or end of file
while(ros::ok() && driver.polling()) {
ros::spinOnce();
}
return 0;
}
sensors/lslidar_m10_driver/lslidar_msgs/CMakeLists.txt
+22
View File
@@ -0,0 +1,22 @@
cmake_minimum_required(VERSION 2.8.3)
project(lslidar_msgs)
find_package(catkin REQUIRED COMPONENTS
message_generation
std_msgs
)
add_message_files(
DIRECTORY msg
FILES
LslidarPacket.msg
LslidarPoint.msg
LslidarScan.msg
LslidarSweep.msg
LslidarDifop.msg
)
generate_messages(DEPENDENCIES std_msgs)
catkin_package(
CATKIN_DEPENDS message_runtime std_msgs
)
sensors/lslidar_m10_driver/lslidar_msgs/msg/LslidarDifop.msg
+1
View File
@@ -0,0 +1 @@
int64 rpm
sensors/lslidar_m10_driver/lslidar_msgs/msg/LslidarPacket.msg
+5
View File
@@ -0,0 +1,5 @@
# Raw Leishen LIDAR packet.
time stamp # packet timestamp
uint8[2000] data # packet contents
sensors/lslidar_m10_driver/lslidar_msgs/msg/LslidarPoint.msg
+12
View File
@@ -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
sensors/lslidar_m10_driver/lslidar_msgs/msg/LslidarScan.msg
+6
View File
@@ -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
sensors/lslidar_m10_driver/lslidar_msgs/msg/LslidarSweep.msg
+4
View File
@@ -0,0 +1,4 @@
Header header
# The 0th scan is at the bottom
LslidarScan[16] scans
sensors/lslidar_m10_driver/lslidar_msgs/package.xml
+20
View File
@@ -0,0 +1,20 @@
<package>
<name>lslidar_msgs</name>
<version>1.2.0</version>
<description>
ROS message definitions for Leishen M10 LIDARs.
</description>
<maintainer email="honghangli@lslidar.com">Nick Li</maintainer>
<author>Nick Li</author>
<license>GNU General Public License V3.0</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>message_generation</build_depend>
<build_depend>std_msgs</build_depend>
<run_depend>message_runtime</run_depend>
<run_depend>std_msgs</run_depend>
</package>
sensors/lslidar_m10_driver/readme.txt
+54
View File
@@ -0,0 +1,54 @@
版本变更
/***************************************************************
初始版本: LSLIDAR_M10_N10_V2.5.0_221103_ROS
变更内容:
1.实现M10/M10_P/M10_PLUS/N10/M10_GPS网口和串口传输数据生成点云功能
2.实现点云角度裁剪和距离过滤功能
3.可以通过lslidar_order话题控制雷达启
4.支持读取pcap包
更改日期: 2022-11-03
***************************************************************/
/***************************************************************
初始版本: LSLIDAR_M10_N10_V2.5.0_221103_ROS
变更版本: LSLIDAR_M10_N10_V2.5.1_221130_ROS
变更内容:
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_ROS
变更版本: LSLIDAR_M10_N10_V2.5.2_230110_ROS
变更内容:
1.日常检测到驱动的包头检测有bug
2.更新M10系列的角度补偿方法。
3.修复发现的点角度不连续bug。
4.添加可选择是否发布pointcloud2话题。
5.添加可选择是否发布scan话题。
6.添加双回波解析方法
7.添加N10_P雷达选择1
8.添加读txt文件功能
更改日期: 2023-01-10
***************************************************************/
/***************************************************************
初始版本: LSLIDAR_M10_N10_V2.5.2_230110_ROS
变更版本: LSLIDAR_M10_N10_V2.5.3_231110_ROS
变更内容:
1.修复M10启停雷达没数据无法启动bug
2.修复雷达停止时驱动CUP占用率过高。
3.添加M10启停雷达停止并且不发数据功能。
4.修复M10P数据错乱导致程序崩溃bug
更改日期: 2023-11-10
***************************************************************/