m5p3nc3r/agv_pro_ros2
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

add Lslidar_ROS2_driver/

Browse Source
This commit is contained in:
X-lanni
2025-06-06 10:44:28 +08:00
parent b2fc659c53
commit d398ba0a22
24 changed files with 3192 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Lslidar_ROS2_driver/lslidar_driver/src/input.cc
+398
View File
@@ -0,0 +1,398 @@
#include "lslidar_driver/input.h"
extern volatile sig_atomic_t flag;
namespace lslidar_driver
{
static const size_t packet_size_input = 400;
////////////////////////////////////////////////////////////////////////
// Input base class implementation
////////////////////////////////////////////////////////////////////////
/** @brief constructor
*
* @param private_nh ROS private handle for calling node.
* @param port UDP port number.
*/
Input::Input(rclcpp::Node *private_nh, uint16_t port) : private_nh_(private_nh), port_(port) {
npkt_update_flag_ = false;
cur_rpm_ = 0;
return_mode_ = 1;
devip_str_difop = std::string("192.168.1.200");
devip_str_ = std::string("192.168.1.102");
lidar_name = std::string("M10");
add_multicast = false;
group_ip = std::string("224.1.1.2");
UDP_PORT_NUMBER_DIFOP = 2369;
private_nh->declare_parameter<std::string>("device_ip","192.168.1.102");
private_nh->declare_parameter<std::string>("device_ip_difop","192.168.1.200");
private_nh->declare_parameter<bool>("add_multicast",false);
private_nh->declare_parameter<std::string>("group_ip","224.1.1.2");
private_nh->declare_parameter<int>("difop_port",2369);
private_nh->get_parameter("lidar_name", lidar_name);
private_nh->get_parameter("device_ip", devip_str_);
private_nh->get_parameter("add_multicast", add_multicast);
private_nh->get_parameter("group_ip", group_ip);
private_nh->get_parameter("difop_port", UDP_PORT_NUMBER_DIFOP);
private_nh->get_parameter("device_ip_difop", devip_str_difop);
if (!devip_str_.empty())
RCLCPP_INFO(private_nh->get_logger(), "[driver][input] accepting packets from IP address: %s port: %d",
devip_str_.c_str(),port);
}
/** @brief constructor
*
* @param private_nh ROS private handle for calling node.
* @param port UDP port number
*/
InputSocket::InputSocket(rclcpp::Node *private_nh, uint16_t port) : Input(private_nh, port) {
sockfd_ = -1;
if (!devip_str_.empty()) {
inet_aton(devip_str_.c_str(), &devip_);
inet_aton(devip_str_difop.c_str(), &devip_difop);
}
RCLCPP_INFO(private_nh_->get_logger(), "[driver][socket] Opening UDP socket: port %d", port);
sockfd_ = socket(PF_INET, SOCK_DGRAM, 0);
if (sockfd_ == -1) {
perror("socket"); // TODO: ROS_ERROR errno
return;
}
int opt = 1;
if (setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (const void *) &opt, sizeof(opt))) {
perror("setsockopt error!\n");
return;
}
sockaddr_in my_addr; // my address information
memset(&my_addr, 0, sizeof(my_addr)); // initialize to zeros
my_addr.sin_family = AF_INET; // host byte order
my_addr.sin_port = htons(port); // port in network byte order
my_addr.sin_addr.s_addr = INADDR_ANY; // automatically fill in my IP
if (bind(sockfd_, (sockaddr * ) & my_addr, sizeof(sockaddr)) == -1) {
perror("bind"); // TODO: ROS_ERROR errno
return;
}
if (add_multicast) {
struct ip_mreq group;
group.imr_multiaddr.s_addr = inet_addr(group_ip.c_str());
group.imr_interface.s_addr = htonl(INADDR_ANY);
if (setsockopt(sockfd_, IPPROTO_IP, IP_ADD_MEMBERSHIP, (char *) &group, sizeof(group)) < 0) {
perror("Adding multicast group error ");
close(sockfd_);
exit(1);
} else
printf("Adding multicast group...OK.\n");
}
if (fcntl(sockfd_, F_SETFL, O_NONBLOCK | FASYNC) < 0) {
perror("non-block");
return;
}
}
/** @brief destructor */
InputSocket::~InputSocket(void) {
(void) close(sockfd_);
}
void Input::UDP_difop()
{
sockaddr_in server_sai;
server_sai.sin_family = AF_INET; // IPV4 协议族
server_sai.sin_port = htons(UDP_PORT_NUMBER_DIFOP);
server_sai.sin_addr.s_addr = inet_addr(devip_str_.c_str());
for (int k = 0; k < 10; k++)
{
unsigned char data[188]= {0x00};
data[0] = 0xA5;
data[1] = 0x5A;
data[2] = 0x55;
data[184] = 0x08;
data[185] = 0x01;
data[186] = 0xFA;
data[187] = 0xFB;
int rtn = sendto(sockfd_, data, 188, 0, (struct sockaddr *)&server_sai, sizeof(struct sockaddr));
if (rtn < 0) printf("start scan error !\n");
else return;
}
return;
}
void Input::UDP_order(const std_msgs::msg::Int8 msg)
{
int i = msg.data;
sockaddr_in server_sai;
server_sai.sin_family = AF_INET; // IPV4 协议族
server_sai.sin_port = htons(UDP_PORT_NUMBER_DIFOP);
server_sai.sin_addr.s_addr = inet_addr(devip_str_.c_str());
int rtn = 0;
for (int k = 0; k < 10; k++)
{
unsigned char data[188]= {0x00};
data[0] = 0xA5;
data[1] = 0x5A;
data[2] = 0x55;
data[186] = 0xFA;
data[187] = 0xFB;
if(lidar_name == "M10" || lidar_name == "M10_GPS" || lidar_name == "M10_P"){
if (i <= 1){ //雷达启停
data[184] = 0x01;
data[185] = char(i);
}
else if (i == 2){ //雷达点云不滤波
data[181] = 0x0A;
data[184] = 0x06;
data[185] = 0x01;
}
else if (i == 3){ //雷达点云正常滤波
data[181] = 0x0B;
data[184] = 0x06;
data[185] = 0x01;
}
else if (i == 4){ //雷达近距离滤波
data[181] = 0x0C;
data[184] = 0x06;
data[185] = 0x01;
}
else if (i == 100){ //接收设备包
data[184] = 0x08;
data[185] = 0x01;
}
else return;
}
else if (lidar_name == "M10_PLUS"){
data[184] = 0x0A;
data[185] = 0x01;
if(i == 5) {
data[141] = 0x01;
data[142] = 0x2c;
}
else if(i == 6) {
data[141] = 0x01;
data[142] = 0x68;
}
else if(i == 8) {
data[141] = 0x01;
data[142] = 0xe0;
}
else if(i == 10) {
data[141] = 0x02;
data[142] = 0x58;
}
else if(i == 12) {
data[141] = 0x02;
data[142] = 0xd0;
}
else if(i == 15) {
data[141] = 0x03;
data[142] = 0x84;
}
else if(i == 20) {
data[141] = 0x04;
data[142] = 0xb0;
}
else if(i <= 1) {
data[184] = 0x01;
data[185] = char(i);
}
else if(i == 100) { //接收设备包
data[184] = 0x08;
data[185] = 0x01;
}
else return;
}
else if(lidar_name == "N10"){
if(i <= 1){
data[185] = char(i);
data[184] = 0x01;
}
else if(i>=6 && i<=12){
data[172] = char(i);
data[184] = 0x0a;
data[185] = 0X01;
}
else return;
}
rtn = sendto(sockfd_, data, 188, 0, (struct sockaddr *)&server_sai, sizeof(struct sockaddr));
if (rtn < 0)
{
printf("start scan error !\n");
}
else
{
if (i == 1)
usleep(3000000);
return;
}
}
return;
}
int InputSocket::getPacket(lslidar_msgs::msg::LslidarPacket::UniquePtr &packet)
{
int q = 0;
struct pollfd fds[1];
fds[0].fd = sockfd_;
fds[0].events = POLLIN;
static const int POLL_TIMEOUT = 2000; // one second (in msec)
sockaddr_in sender_address{};
socklen_t sender_address_len = sizeof(sender_address);
while (flag == 1)
{
// poll() until input available
do {
int retval = poll(fds, 1, POLL_TIMEOUT);
if (retval < 0) // poll() error?
{
if (errno != EINTR)
RCLCPP_ERROR(private_nh_->get_logger(), "[driver][socket] poll() error: %s", strerror(errno));
return 0;
}
if (retval == 0) // poll() timeout?
{
RCLCPP_WARN(private_nh_->get_logger(), "lslidar poll() timeout, port: %d",port_);
return 0;
}
if ((fds[0].revents & POLLERR) || (fds[0].revents & POLLHUP) || (fds[0].revents & POLLNVAL)) // device error?
{
RCLCPP_ERROR(private_nh_->get_logger(),"poll() reports lslidar error");
return 0;
}
} while ((fds[0].revents & POLLIN) == 0);
// Receive packets that should now be available from the
// socket using a blocking read.
ssize_t nbytes = recvfrom(sockfd_, &packet->data[0], packet_size_input, 0,
(sockaddr *)&sender_address, &sender_address_len);
// ROS_DEBUG_STREAM("incomplete lslidar packet read: "
// << nbytes << " bytes");
q = (int)nbytes;
if (nbytes < 0)
{
if (errno != EWOULDBLOCK)
{
perror("recvfail");
RCLCPP_ERROR(private_nh_->get_logger(),"recvfail");
return 1;
}
}
else if ((size_t)nbytes <= packet_size_input || (size_t)nbytes >= 50)
{
// read successful,
// if packet is not from the lidar scanner we selected by IP,
// continue otherwise we are done
if (devip_str_ != "" && sender_address.sin_addr.s_addr != devip_.s_addr)
continue;
else
break; // done
}
}
if (flag == 0)
{
abort();
}
return q;
}
InputPCAP::InputPCAP(rclcpp::Node *private_nh, uint16_t port, double packet_rate, std::string filename) : Input(private_nh, port),
packet_rate_(packet_rate),
filename_(filename)
{
pcap_ = NULL;
empty_ = true;
read_once_ = false;
read_fast_ = false;
repeat_delay_ = 0.0;
private_nh->get_parameter("read_once", read_once_);
private_nh->get_parameter("read_fast", read_fast_);
private_nh->get_parameter("repeat_delay", repeat_delay_);
if (read_once_)
RCLCPP_WARN(private_nh_->get_logger(),"Read input file only once.");
if (read_fast_)
RCLCPP_WARN(private_nh_->get_logger(),"Read input file as quickly as possible.");
if (repeat_delay_ > 0.0)
RCLCPP_WARN(private_nh_->get_logger(),"Delay %.3f seconds before repeating input file.", repeat_delay_);
RCLCPP_INFO(private_nh_->get_logger(),"Opening PCAP file %s",filename_.c_str());
if ((pcap_ = pcap_open_offline(filename_.c_str(), errbuf_)) == NULL)
{
RCLCPP_WARN(private_nh_->get_logger(),"Error opening lslidar socket dump file.");
return;
}
std::stringstream filter;
if (devip_str_ != "")
{
filter << "src host " << devip_str_ << "&&";
}
filter << "udp dst port " << port;
pcap_compile(pcap_, &pcap_packet_filter_, filter.str().c_str(), 1, PCAP_NETMASK_UNKNOWN);
}
InputPCAP::~InputPCAP(void)
{
pcap_close(pcap_);
}
int InputPCAP::getPacket(lslidar_msgs::msg::LslidarPacket::UniquePtr &pkt)
{
struct pcap_pkthdr *header;
const u_char *pkt_data;
while (flag == 1)
{
int res;
if ((res = pcap_next_ex(pcap_, &header, &pkt_data)) >= 0)
{
// skip packets not for the correct port and from the selected IP address
if (!devip_str_.empty() && (0 == pcap_offline_filter(&pcap_packet_filter_, header, pkt_data)))
continue;
if (read_fast_ == false)
packet_rate_.sleep();
mempcpy(&pkt->data[0], pkt_data + 42, packet_size_input);
empty_ = false;
return 0;
}
if (empty_)
{
RCLCPP_WARN(private_nh_->get_logger(),"Error %d reading lslidar packet: %s", res, pcap_geterr(pcap_));
return -1;
}
if (read_once_)
{
RCLCPP_WARN(private_nh_->get_logger(),"end of file reached -- done reading.");
return -1;
}
if (repeat_delay_ > 0.0)
{
RCLCPP_WARN(private_nh_->get_logger(),"end of file reached -- delaying %.3f seconds.", repeat_delay_);
usleep(rint(repeat_delay_ * 1000000.0));
}
RCLCPP_WARN(private_nh_->get_logger(),"replayding lsliar dump file");
pcap_close(pcap_);
pcap_ = pcap_open_offline(filename_.c_str(), errbuf_);
empty_ = true;
}
if (flag == 0)
{
abort();
}
return 0;
}
} // namespace
Lslidar_ROS2_driver/lslidar_driver/src/lsiosr.cpp
+400
View File
@@ -0,0 +1,400 @@
/*******************************************************
@company: Copyright (C) 2022, Leishen Intelligent System
@product: LSM10 and N10
@filename: lsiosr.cpp
@brief:
@version: date: author: comments:
@v1.0 21-2-4 yao new
*******************************************************/
#include "lslidar_driver/lsiosr.h"
namespace lslidar_driver {
LSIOSR * LSIOSR::instance(std::string name, int speed, int fd)
{
static LSIOSR obj(name, speed, fd);
return &obj;
}
LSIOSR::LSIOSR(std::string port, int baud_rate, int fd):port_(port), baud_rate_(baud_rate), fd_(fd)
{
printf("port = %s, baud_rate = %d\n", port.c_str(), baud_rate);
}
LSIOSR::~LSIOSR()
{
close();
}
/* 串口配置的函数 */
int LSIOSR::setOpt(int nBits, uint8_t nEvent, int nStop)
{
struct termios newtio, oldtio;
/*保存测试现有串口参数设置，在这里如果串口号等出错，会有相关的出错信息*/
if (tcgetattr(fd_, &oldtio) != 0)
{
perror("SetupSerial 1");
return -1;
}
bzero(&newtio, sizeof(newtio));
/*步骤一，设置字符大小*/
newtio.c_cflag |= CLOCAL; //如果设置，modem 的控制线将会被忽略。如果没有设置，则 open()函数会阻塞直到载波检测线宣告 modem 处于摘机状态为止。
newtio.c_cflag |= CREAD; //使端口能读取输入的数据
/*设置每个数据的位数*/
switch (nBits)
{
case 7:
newtio.c_cflag |= CS7;
break;
case 8:
newtio.c_cflag |= CS8;
break;
}
/*设置奇偶校验位*/
switch (nEvent)
{
case 'O': //奇数
newtio.c_iflag |= (INPCK | ISTRIP);
newtio.c_cflag |= PARENB; //使能校验，如果不设PARODD则是偶校验
newtio.c_cflag |= PARODD; //奇校验
break;
case 'E': //偶数
newtio.c_iflag |= (INPCK | ISTRIP);
newtio.c_cflag |= PARENB;
newtio.c_cflag &= ~PARODD;
break;
case 'N': //无奇偶校验位
newtio.c_cflag &= ~PARENB;
break;
}
/*设置波特率*/
switch (baud_rate_)
{
case 230400:
cfsetispeed(&newtio, B230400);
cfsetospeed(&newtio, B230400);
break;
case 460800:
cfsetispeed(&newtio, B460800);
cfsetospeed(&newtio, B460800);
break;
case 500000:
cfsetispeed(&newtio, B500000);
cfsetospeed(&newtio, B500000);
break;
case 921600:
cfsetispeed(&newtio, B921600);
cfsetospeed(&newtio, B921600);
break;
default:
cfsetispeed(&newtio, B460800);
cfsetospeed(&newtio, B460800);
break;
}
/*
* 设置停止位
* 设置停止位的位数， 如果设置，则会在每帧后产生两个停止位， 如果没有设置，则产生一个
* 停止位。一般都是使用一位停止位。需要两位停止位的设备已过时了。
* */
if (nStop == 1)
newtio.c_cflag &= ~CSTOPB;
else if (nStop == 2)
newtio.c_cflag |= CSTOPB;
/*设置等待时间和最小接收字符*/
newtio.c_cc[VTIME] = 0;
newtio.c_cc[VMIN] = 0;
/*处理未接收字符*/
tcflush(fd_, TCIFLUSH);
/*激活新配置*/
if ((tcsetattr(fd_, TCSANOW, &newtio)) != 0)
{
perror("serial set error");
return -1;
}
return 0;
}
void LSIOSR::flushinput() {
tcflush(fd_, TCIFLUSH);
}
/* 从串口中读取数据 */
int LSIOSR::read(unsigned char *buffer, int length, int timeout)
{
memset(buffer, 0, length);
int totalBytesRead = 0;
int rc;
int unlink = 0;
unsigned char* pb = buffer;
if (timeout > 0)
{
rc = waitReadable(timeout);
if (rc <= 0)
{
return (rc == 0) ? 0 : -1;
}
int retry = 3;
while (length > 0)
{
rc = ::read(fd_, pb, (size_t)length);
if (rc > 0)
{
length -= rc;
pb += rc;
totalBytesRead += rc;
if (length == 0)
{
break;
}
}
else if (rc < 0)
{
printf("error \n");
retry--;
if (retry <= 0)
{
break;
}
}
unlink++;
rc = waitReadable(20);
if(unlink > 10)
return -1;
if (rc <= 0)
{
break;
}
}
}
else
{
rc = ::read(fd_, pb, (size_t)length);
if (rc > 0)
{
totalBytesRead += rc;
}
else if ((rc < 0) && (errno != EINTR) && (errno != EAGAIN))
{
printf("read error\n");
return -1;
}
}
return totalBytesRead;
}
int LSIOSR::waitReadable(int millis)
{
if (fd_ < 0)
{
return -1;
}
int serial = fd_;
fd_set fdset;
struct timeval tv;
int rc = 0;
while (millis > 0)
{
if (millis < 5000)
{
tv.tv_usec = millis % 1000 * 1000;
tv.tv_sec = millis / 1000;
millis = 0;
}
else
{
tv.tv_usec = 0;
tv.tv_sec = 5;
millis -= 5000;
}
FD_ZERO(&fdset);
FD_SET(serial, &fdset);
rc = select(serial + 1, &fdset, NULL, NULL, &tv);
if (rc > 0)
{
rc = (FD_ISSET(serial, &fdset)) ? 1 : -1;
break;
}
else if (rc < 0)
{
rc = -1;
break;
}
}
return rc;
}
int LSIOSR::waitWritable(int millis)
{
if (fd_ < 0)
{
return -1;
}
int serial = fd_;
fd_set fdset;
struct timeval tv;
int rc = 0;
while (millis > 0)
{
if (millis < 5000)
{
tv.tv_usec = millis % 1000 * 1000;
tv.tv_sec = millis / 1000;
millis = 0;
}
else
{
tv.tv_usec = 0;
tv.tv_sec = 5;
millis -= 5000;
}
FD_ZERO(&fdset);
FD_SET(serial, &fdset);
rc = select(serial + 1, NULL, &fdset, NULL, &tv);
if (rc > 0)
{
rc = (FD_ISSET(serial, &fdset)) ? 1 : -1;
break;
}
else if (rc < 0)
{
rc = -1;
break;
}
}
return rc;
}
/* 向串口中发送数据 */
int LSIOSR::send(const char* buffer, int length, int timeout)
{
if (fd_ < 0)
{
return -1;
}
if ((buffer == 0) || (length <= 0))
{
return -1;
}
int totalBytesWrite = 0;
int rc;
char* pb = (char*)buffer;
if (timeout > 0)
{
rc = waitWritable(timeout);
if (rc <= 0)
{
return (rc == 0) ? 0 : -1;
}
int retry = 3;
while (length > 0)
{
rc = write(fd_, pb, (size_t)length);
if (rc > 0)
{
length -= rc;
pb += rc;
totalBytesWrite += rc;
if (length == 0)
{
break;
}
}
else
{
retry--;
if (retry <= 0)
{
break;
}
}
rc = waitWritable(50);
if (rc <= 0)
{
break;
}
}
}
else
{
rc = write(fd_, pb, (size_t)length);
if (rc > 0)
{
totalBytesWrite += rc;
}
else if ((rc < 0) && (errno != EINTR) && (errno != EAGAIN))
{
return -1;
}
}
return totalBytesWrite;
}
int LSIOSR::init()
{
int error_code = 0;
fd_ = open(port_.c_str(), O_RDWR|O_NOCTTY|O_NDELAY);
if (0 < fd_)
{
error_code = 0;
setOpt(DATA_BIT_8, PARITY_NONE, STOP_BIT_1);//设置串口参数
//printf("open_port %s OK !\n", port_.c_str());
}
else
{
error_code = -1;
}
return error_code;
}
int LSIOSR::close()
{
::close(fd_);
return 0;
}
std::string LSIOSR::getPort()
{
return port_;
}
int LSIOSR::setPortName(std::string name)
{
port_ = name;
return 0;
}
}
Lslidar_ROS2_driver/lslidar_driver/src/lslidar_driver.cc
+1383
View File
File diff suppressed because it is too large Load Diff
Lslidar_ROS2_driver/lslidar_driver/src/lslidar_driver_node.cc
+35
View File
@@ -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;
}