|
|
@@ -0,0 +1,205 @@
|
|
|
+# 1.可视化雷达点云-学会驱动雷达
|
|
|
+
|
|
|
+在购买FishBot送的快速上手教程中，直接使用 Docker 来运行了雷达和直接建图，本章我们将介绍从源码的方式驱动雷达和完成建图和导航配置，本节我们来学习如何驱动雷达。
|
|
|
+
|
|
|
+## 一、激光雷达介绍
|
|
|
+
|
|
|
+激光雷达（Light Detection And Ranging）,缩写`LiDAR`，翻译一下叫——激光探测与测距。
|
|
|
+
|
|
|
+### 1.1 激光雷达原理介绍
|
|
|
+
|
|
|
+激光雷达的原理也很简单，就像蝙蝠的定位方法一样，蝙蝠定位大家都知道吧，像下面这样子的回声定位。
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+普通的单线激光雷达一般有一个发射器，一个接收器，发射器发出激光射线到前方的目标上，物品会将激光反射回来，然后激光雷达的接受器可以检测到反射的激光。
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+通过计算发送和反馈之间的时间间隔，乘上激光的速度，就可以计算出激光飞行的距离，该计算方法成为TOF（飞行时间法Time of flight，也称时差法）。
|
|
|
+
|
|
|
+除了TOF之外还有其他方法进行测距，比如三角法，这里就不拓展了放一篇文章，大家自行阅读。
|
|
|
+
|
|
|
+- [激光三角测距原理详述](https://www.slamtec.com/cn/News/Detail/190)
|
|
|
+
|
|
|
+目前市面上的激光雷达，几乎都是采用三角测距，比如思岚的：
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+**需要注意的是虽然只有一个发射器和一个接受器，激光雷达通过电机可以进行旋转，这样就可以达到对周围环境360度测距的目的。**
|
|
|
+
|
|
|
+## 二、驱动雷达
|
|
|
+
|
|
|
+目前FishBot主要配套雷达型号为EAI-X2，后续可能会引入其他雷达。
|
|
|
+
|
|
|
+针对EAI-X2，采用的驱动为小鱼修改后的ROS2驱动:
|
|
|
+- 代码仓库为：https://github.com/fishros/ydlidar_ros2
|
|
|
+- 代码分支为：v1.0.0/fishbot
|
|
|
+
|
|
|
+### 2.1 有线驱动-树莓派工控机直连都可以通过这种方式
|
|
|
+
|
|
|
+将配套的雷达转接板模式调整到UART模式，拔掉EN跳线帽（可以关闭ESP8266,节省能源）， 找一根USB线，将 雷达板接入到你要驱动的电脑或者各种PI上。
|
|
|
+
|
|
|
+下载源码到任意一个目录：
|
|
|
+```
|
|
|
+git clone http://github.fishros.org/https://github.com/fishros/ydlidar_ros2 -b v1.0.0/fishbot
|
|
|
+```
|
|
|
+进入到源码，修改串口编号ydlidar_ros2/params/ydlidar.yaml，一般是/dev/ttyUSB0
|
|
|
+```
|
|
|
+ydlidar_node:
|
|
|
+ ros__parameters:
|
|
|
+ port: /dev/ttyUSB0
|
|
|
+ frame_id: laser_frame
|
|
|
+ ignore_array: ""
|
|
|
+```
|
|
|
+接着编译 ：
|
|
|
+```
|
|
|
+cd ydlidar_ros2
|
|
|
+colcon build
|
|
|
+```
|
|
|
+
|
|
|
+接着修改串口权限，然后运行驱动
|
|
|
+```
|
|
|
+sudo chmod 666 /dev/ttyUSB0
|
|
|
+source install/setup.bash
|
|
|
+ros2 launch ydlidar ydlidar_launch.py
|
|
|
+---
|
|
|
+[INFO] [launch]: All log files can be found below /home/pi/.ros/log/2023-07-21-23-13-28-893425-raspberrypi-4518
|
|
|
+[INFO] [launch]: Default logging verbosity is set to INFO
|
|
|
+[INFO] [ydlidar_node-1]: process started with pid [4539]
|
|
|
+[INFO] [static_transform_publisher-2]: process started with pid [4541]
|
|
|
+[static_transform_publisher-2] [WARN] [1689952409.891692804] []: Old-style arguments are deprecated; see --help for new-style arguments
|
|
|
+[static_transform_publisher-2] [INFO] [1689952409.975433434] [static_tf_pub_laser]: Spinning until stopped - publishing transform
|
|
|
+[static_transform_publisher-2] translation: ('0.020000', '0.000000', '0.000000')
|
|
|
+[static_transform_publisher-2] rotation: ('0.000000', '0.000000', '0.000000', '1.000000')
|
|
|
+[static_transform_publisher-2] from 'base_link' to 'laser_frame'
|
|
|
+[ydlidar_node-1] [YDLIDAR INFO] Current ROS Driver Version: 1.4.5
|
|
|
+[ydlidar_node-1] [YDLIDAR]:SDK Version: 1.4.5
|
|
|
+[ydlidar_node-1] [YDLIDAR]:Lidar running correctly ! The health status: good
|
|
|
+[ydlidar_node-1] [YDLIDAR] Connection established in [/dev/ttyUSB0][115200]:
|
|
|
+[ydlidar_node-1] Firmware version: 1.5
|
|
|
+[ydlidar_node-1] Hardware version: 1
|
|
|
+[ydlidar_node-1] Model: S4
|
|
|
+[ydlidar_node-1] Serial: 2020112400007024
|
|
|
+[ydlidar_node-1] [YDLIDAR]:Fixed Size: 370
|
|
|
+[ydlidar_node-1] [YDLIDAR]:Sample Rate: 3K
|
|
|
+[ydlidar_node-1] [YDLIDAR INFO] Current Sampling Rate : 3K
|
|
|
+[ydlidar_node-1] [YDLIDAR INFO] Now YDLIDAR is scanning ......
|
|
|
+```
|
|
|
+
|
|
|
+最后使用ros2 topic list 就可以看到话题list了，scan就是雷达话题
|
|
|
+```
|
|
|
+ros2 topic list
|
|
|
+---
|
|
|
+/parameter_events
|
|
|
+/rosout
|
|
|
+/scan
|
|
|
+/tf_static
|
|
|
+/ydlidar_node/transition_event
|
|
|
+```
|
|
|
+
|
|
|
+### 2.2 无线驱动
|
|
|
+
|
|
|
+如果想通过源码的方式也可以，**雷达板的主要作用是将雷达 数据生成一个虚拟的串口**，这样就相当于有线连接了。
|
|
|
+
|
|
|
+首先要给雷达板烧录固件，这个固件的作用就是将串口转成无线TCP,所以对于这段固件的开发，没有要学习的知识点，直接使用即可，固件烧录及配置方式如下：https://www.fishros.org.cn/forum/topic/940/fishbot%E6%95%99%E7%A8%8B-5-%E9%9B%B7%E8%BE%BE%E5%9B%BA%E4%BB%B6%E7%83%A7%E5%BD%95%E5%8F%8A%E9%85%8D%E7%BD%AE/1?lang=zh-CN
|
|
|
+
|
|
|
+烧录完成固件后，接着我们就可以启动服务让雷达板连接上来，在电脑上直接运行下面这段Python代码，当连接建立后，就会直接生成 一个虚拟串口，然后按照 2.1 的方式就可以直接驱动了。
|
|
|
+
|
|
|
+```
|
|
|
+#!/usr/bin/env python3
|
|
|
+
|
|
|
+import subprocess
|
|
|
+import os
|
|
|
+import pty
|
|
|
+import socket
|
|
|
+import select
|
|
|
+import argparse
|
|
|
+import subprocess
|
|
|
+import time
|
|
|
+
|
|
|
+class LaserScanRos2():
|
|
|
+
|
|
|
+ def __init__(self) -> None:
|
|
|
+ self.laser_pro = None
|
|
|
+
|
|
|
+
|
|
|
+class SocketServer():
|
|
|
+ def __init__(self,lport=8889,uart_name="/tmp/fishbot_laser") -> None:
|
|
|
+ self.lport = lport
|
|
|
+ self.uart_name = uart_name
|
|
|
+ self.laser_ros2 = LaserScanRos2()
|
|
|
+ self.main()
|
|
|
+
|
|
|
+ def main(self):
|
|
|
+ s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
|
|
|
+ s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
|
|
|
+ s.bind(('0.0.0.0', self.lport))
|
|
|
+ s.listen(5)
|
|
|
+ master, slave = pty.openpty()
|
|
|
+ if os.path.exists(self.uart_name):
|
|
|
+ os.remove(self.uart_name)
|
|
|
+ os.symlink(os.ttyname(slave), self.uart_name)
|
|
|
+ print(f"UART2SOCKET:{self.lport}->{self.uart_name}")
|
|
|
+ mypoll = select.poll()
|
|
|
+ mypoll.register(master, select.POLLIN)
|
|
|
+ try:
|
|
|
+ while True:
|
|
|
+ print("Prepare to Accept connect!")
|
|
|
+ client, client_address = s.accept()
|
|
|
+ mypoll.register(client.fileno(), select.POLLIN)
|
|
|
+ print(s.fileno(), client, master)
|
|
|
+ print('PTY: Opened {} for {}:{}'.format(
|
|
|
+ os.ttyname(slave), '0.0.0.0', self.lport))
|
|
|
+ is_connect = True
|
|
|
+ try:
|
|
|
+ while is_connect:
|
|
|
+ fdlist = mypoll.poll(256)
|
|
|
+ for fd, event in fdlist:
|
|
|
+ data = os.read(fd, 256)
|
|
|
+ write_fd = client.fileno() if fd == master else master
|
|
|
+ if len(data) == 0:
|
|
|
+ is_connect = False
|
|
|
+ break
|
|
|
+ os.write(write_fd, data)
|
|
|
+ # print(fd, event, data)
|
|
|
+ except ConnectionResetError:
|
|
|
+ is_connect = False
|
|
|
+ print("远程被迫断开链接")
|
|
|
+ finally:
|
|
|
+ mypoll.unregister(client.fileno())
|
|
|
+ finally:
|
|
|
+ s.close()
|
|
|
+ os.close(master)
|
|
|
+ os.close(slave)
|
|
|
+ os.remove(self.uart_name)
|
|
|
+
|
|
|
+def main():
|
|
|
+ SocketServer()
|
|
|
+
|
|
|
+
|
|
|
+if __name__ == "__main__":
|
|
|
+ main()
|
|
|
+
|
|
|
+```
|
|
|
+
|
|
|
+连接成功后，会生成/tmp/fishbot_laser 虚拟串口，修改雷达驱动的端口号到这个虚拟串口，确定雷达连接上，然后启动雷达驱动就可以了。
|
|
|
+
|
|
|
+```
|
|
|
+python3 test.py
|
|
|
+---
|
|
|
+UART2SOCKET:8889->/tmp/fishbot_laser
|
|
|
+Prepare to Accept connect!
|
|
|
+3 <socket.socket fd=6, family=AddressFamily.AF_INET, type=SocketKind.SOCK_STREAM, proto=0, laddr=('192.168.4.136', 8889), raddr=('192.168.4.49', 52843)> 4
|
|
|
+PTY: Opened /dev/pts/4 for 0.0.0.0:8889
|
|
|
+```
|
|
|
+
|
|
|
+## 三、可视化雷达数据
|
|
|
+
|
|
|
+雷达驱动成功后，可以用RVIV2可视化雷达数据。在终端中输入rviz2,然后修改fixedframe为雷达驱动配置文件中的` frame_id: laser_frame`，接着通过话题添加可视化模块。因为雷达也属于传感器，所以需要修改QOS部分 可靠政策为 Best Effort，具体配置如下：
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+最终现实效果如下
|
|
|
+
|
|
|
+
|
鱼香ROS
