add Lslidar_ROS2_driver/
This commit is contained in:
X-lanni
@@ -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()
|
||||
@@ -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
|
||||
@@ -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
|
||||
|
||||
@@ -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_
|
||||
@@ -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,
|
||||
])
|
||||
|
||||
@@ -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,
|
||||
])
|
||||
|
||||
@@ -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>
|
||||
@@ -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文件读取功能
|
||||
@@ -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文件读取功能
|
||||
@@ -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文件读取功能
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
@@ -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;
|
||||
}
|
||||
Reference in New Issue
Block a user