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