效果展示

话不多说,先上激光雷达数据显示效果图:

python 获取雷达数据 雷达数据读取_串口


激光雷达用的是雷达大叔的二手货,基本上长这个样子:

python 获取雷达数据 雷达数据读取_python 获取雷达数据_02


操作步骤

要生成激光雷达图像,首先就是获取串口数据了,所以第一步就是读取串口数据:

由于是帮朋友调试,开始朋友给了错误的SDK,导致调试半天没有任何结果,索性用串口助手查看一下这货返回些什么数据:

python 获取雷达数据 雷达数据读取_C#_03

眼神要好,有一列FA,没错,就是250,看样子是帧头,查询一下这款雷达的数据格式:

<start> <index> <speed_l> <speed_h> [Data 0] [Data 1] [Data 2] [Data 3] <checksum_l> <checksum_h> 
<start>:0xFA是固定格式表明数据包开始,可用来从数据流中分割数据包
<index>:数据包的索引号,范围从0xA0 到 0xF9 (每个包4个数据)。
<speed>:有两个speedL和speedH ,它们各一个字节,共同组成转速信息。
<data>:[ Data 0] 到 [Data 3] 是四组测量数据,其中每组测量数据分别由4个字节组成,如下: 
byte 0 : <distance 7:0="">  # 距离信息的0-7位 
byte 1 : <“invalid data” flag> <“strength warning” flag> <distance 13:8=""> # 错误信息标志位 , 警告位, 距离信息13-8位
byte 2 : <signal strength="" 7:0="">  # 信号强度 0-7位 
byte 3 : <signal strength="" 15:8=""> # 讯号强度 8-15位 
距离信息的单位是mm ,整个激光雷达的测量范围大概是15cm 到6m, 只需要把距离信息的两个字节组装成一个整数即可(注意判断无效数据位为0,如果为1意味是无效的数据,需丢弃)。 
 <checksum>:由两个字节组成的校验码,用来校验整个数据包是否正确。

一、读取串口数据

有了上述信息之后,首先编写基本串口类,用来打开串口、读取数据:

有以下2点值得注意:

1. 使用DataReceived事件来读取数据;

2. 使用isReading和isClosing标识来保证正常关闭激光雷达(防止出现读取已关闭串口数据的情况);

还有就是,可以定义一个委托,用来读取完数据后,执行处理操作;

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.IO;
using System.IO.Ports;

namespace Laser {
    public class MPort {
        ///
        /// Attribute(属性域)
        ///
        // 串口实例
        private SerialPort port;
        // 串口名和串口波特率
        private String portName = "COM3";
        private int baudRate = 115200;
        // 正在关闭、正在读取串口状态
        private bool isClosing;
        private bool isReading;
        // 串口返回数据
        private byte[] receData = new byte[2048];
        private Object receLock = new object();
        // 定义一个委托
        public delegate void processData(byte[] receData);
        public processData processDel;

        ///
        /// Attribute Modify(属性域修改)
        ///
        /// <summary>
        /// 串口名
        /// </summary>
        public String PortName{
            get{ return portName;}
            set{ portName = value;}
        }
        /// <summary>
        /// 波特率
        /// </summary>
        public int BaudRate{
            get{ return baudRate;}
            set{ baudRate = value;}
        }
        /// <summary>
        /// 读取数据
        /// </summary>
        public byte[] ReceData {
            get {
                lock (receLock) {
                    return receData;
                }
            }
            set {
                lock (receLock) {
                    receData = value;
                }
            }
        }
        /// <summary>
        /// 串口打开状态
        /// </summary>
        public bool IsOpen {
            get {
                return port.IsOpen;
            }
        }

        ///
        /// Method(方法域)
        ///
        /// <summary>
        /// 串口实例构造方法
        /// </summary>
        public MPort(String portName, int baudRate = 115200) {
            try {
                // 尝试关闭已使用的串口
                if (port != null) {
                    this.closePort();
                }
                // 尝试打开新串口
                this.PortName = portName;
                this.BaudRate = baudRate;
                port = new SerialPort(this.PortName, this.BaudRate);
            }
            catch {
                // "Check your port!"
            }
        }
        /// <summary>
        /// 打开串口方法
        /// </summary>
        /// <returns>打开是否成功</returns>
        public bool openPort() {
            // 注册读取串口事件
            port.DataReceived -= port_DataReceived;
            port.DataReceived += port_DataReceived;
            // 串口未打开时尝试打开串口
            if (!port.IsOpen) {
                isClosing = false;
                try {
                    port.Open();
                }
                catch (Exception ex) {
                    Console.WriteLine(ex.Message);
                    return false;
                }
            }
            return true;
        }
        /// <summary>
        /// 关闭串口方法
        /// </summary>
        /// <returns>关闭串口是否成功</returns>
        public bool closePort() {
            if (!port.IsOpen) {
                return true;
            }
            isClosing = true;
            // 等待读取事件完毕
            while (isReading) {
                System.Windows.Forms.Application.DoEvents();
            }
            // 尝试关闭串口
            try {
                port.Close();
            }
            catch {
                return false;
            }
            return true;
        }
        

        /// <summary>
        /// 读取串口事件
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        void port_DataReceived(object sender, SerialDataReceivedEventArgs e) {
            // 正在关闭则返回
            if (isClosing) {
                return;
            }
            // 设置正在读取状态
            isReading = true;
            try {
                int count = Math.Min(port.BytesToRead, ReceData.Length);
                port.Read(ReceData, 0, count);
                // 委托方法不为空,调用委托方法
                if (processDel != null) {
                    processDel(ReceData);
                }
            }
            finally {
                isReading = false;
            }
            
        }

        /// <summary>
        /// 向串口写入数据
        /// </summary>
        /// <param name="buff">写入数据的内容</param>
        /// <returns>是否写入成功的返回值</returns>
        public bool writeData(byte[] buff) {
            // 尝试向串口写入数据
            try {
                port.Write(buff, 0, buff.Length);
            }
            catch (Exception ex) {
                Console.WriteLine(ex.Message);
                return false;
            }
            return true;
        }

    }
}

上述代码完成后,基本的串口操作就可以通过这个类完成,如果要对激光雷达做处理,则在次类基础上继承一个激光雷达类即可:

二、处理激光雷达数据

1. 继承MPort类,生成激光雷达类;

2. 考虑到当前只有到一个激光雷达,采用单例模式设计;

3. 绑定ProcessData方法,以便读取完数据后进行处理;

4. 数据处理是根据数据格式定义来编写的;

5. 预留了一个处理后的委托;

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace Laser {
    public class Radar: MPort {
        ///
        /// Attribute(属性域)
        ///
        // 距离信息
        private int[] dist = new int[360];
        // 强度信息
        private int[] back = new int[360];
        // 旋转速度
        private int speed;
        // 读写锁
        private Object readLock = new object();

        ///
        /// Attribute Modify(属性域修改)
        ///
        /// <summary>
        /// 距离信息
        /// </summary>
        public int[] Dist {
            get {
                lock (readLock) {
                    return dist;
                }
            }
            set {
                lock (readLock) {
                    dist = value;
                }
            }
        }
        /// <summary>
        /// 强度信息
        /// </summary>
        public int[] Back {
            get {
                lock (readLock) {
                    return back;
                }
            }
            set {
                lock (readLock) {
                    back = value;
                }
            }
        }
        /// <summary>
        /// 速度信息
        /// </summary>
        public int Speed {
            get {
                lock (readLock){
                    return speed;
                }
            }
            set {
                lock (readLock) {
                    speed = value;
                }
            }
        }

        ///
        /// Method(方法域)
        ///
        private Radar(String portName, int baudRate = 115200):base(
            portName, baudRate) {
                this.processDel -= new processData(this.processData);
                this.processDel += new processData(this.processData);
        }

        private static Radar radar;
        /// <summary>
        /// 单例模式,创建激光雷达类
        /// </summary>
        /// <param name="portName">串口号</param>
        /// <param name="baudRate">波特率</param>
        /// <returns>激光雷达实例</returns>
        public static Radar getInstance(String portName, int baudRate = 115200) {
            if (radar == null || !portName.Equals(radar.PortName)) {
                radar = new Radar(portName, baudRate);
            }
            return radar;
        }

        /// <summary>
        /// 数据头
        /// </summary>
        public static byte dHead = (byte)0xFA;
        /// <summary>
        /// 数据帧长度
        /// </summary>
        public static int dLen = 22;
        /// <summary>
        /// 处理激光雷达获取的数据信息
        /// </summary>
        /// <param name="data">获取的数据</param>
        public void processData(byte[] data) {
            // 该数据完整则进行处理
            for (int i = 0; i < data.Length && (i + dLen - 1 < data.Length); ) {
                // 寻找帧头
                if (data[i] != dHead) {
                    ++i;
                    continue;
                }
                // 索引号
                int index = data[i + 1];
                Speed = data[i + 2] + 256 * (data[i + 3]);
                // 计算距离和强度信息
                for (int j = 0; j < 4; ++j) {
                    int tag = (index - 160) * 4 + j;
                    if (tag < 0 || tag >= 360) {
                        break;
                    }
                    Dist[tag] = data[i + 3 + j * 4 + 1] + 256 * (data[i + 3 + j * 4 + 2] & 0x3F);
                    Back[tag] = data[i + 3 + j * 4 + 3] + 256 * data[i + 3 + j * 4 + 4];
                }
                i += dLen;
            }
            // 执行数据处理完成后的方法
            if (afterProcessDel != null) {
                afterProcessDel();
            }
        }
        /// <summary>
        /// 定义数据处理后执行方法
        /// </summary>
        public delegate void afterProcess();
        public static afterProcess afterProcessDel;

    }
}

三、绘图显示

为了进行显示,需要把从串口读到的激光雷达距离信息和强度信息画在图上,其中,画图使用winForm的Graphic,

1. 创建一张Bitmap;

2. 根据角度距离信息,计算当前测量点的坐标;

3. 在坐标处绘制一个点,其实就是实心圆,使用FillEclipse方法;

4. 此处画图的坐标比较神奇,是因为坐标原点在左上角(0,0),向下为Y轴增大方向,向右为X轴增大方向;

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Drawing;

namespace Laser {
    public class DrawRadarImg {
        ///
        /// Attribute(属性域)
        ///
        // 图像宽度
        private int width;
        // 图像高度
        private int height;
        // 显示比例
        private double rate = 0.05;
        // 像素点大小
        private static int ps = 4;

        ///
        /// Attribute Modify(属性域修改)
        ///
        /// <summary>
        /// 图像宽度
        /// </summary>
        public int Width {
            get { return width; }
            set { width = value; }
        }
        /// <summary>
        /// 图像高度
        /// </summary>
        public int Height {
            get { return height; }
            set { height = value; }
        }
        /// <summary>
        /// 长度比例
        /// </summary>
        public double Rate {
            get { return rate; }
            set { rate = value; }
        }
        /// <summary>
        /// 一个测量点大小
        /// </summary>
        public int PS {
            get { return ps; }
            set { ps = value; }
        }

        ///
        /// Method(方法域)
        ///
        /// <summary>
        /// 根据距离和强度信息画图
        /// </summary>
        /// <param name="dist">距离信息</param>
        /// <param name="back">强度信息</param>
        /// <returns></returns>
        public Image drawImg(int[] dist, int[] back) {
            Bitmap img = new Bitmap(400, 400);
            Graphics g = Graphics.FromImage(img);
            for (int i = 0; i < 360; ++i) {
                // 计算当前角度、X、Y坐标(偏差90度,与设定相关)
                double ang = ((i + 90) / 180.0) * Math.PI;
                double x = Math.Cos(ang) * dist[i] * rate;
                double y = Math.Sin(ang) * dist[i] * rate;
                // 调整强度显示的颜色
                Brush brush = (back[i] > 300) ? (Brushes.Red) :
                    (back[i] > 200) ? (Brushes.Green) :
                    (back[i] > 100) ? (Brushes.Blue) : Brushes.Purple;
                // 画点
                g.FillEllipse(brush, (int)(x + 200 - ps / 2), (int)(200 - (y - ps / 2)), ps, ps);
            }
            return img;
        }
    }
}

最后,将这些类在Form1中进行调用,代码如下:

using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;

namespace Laser {
    public partial class Form1 : Form {

        // 激光雷达对象
        private Radar radar;
        private DrawRadarImg dImg = new DrawRadarImg();

        public Form1() {
            InitializeComponent();

            // 绑定更新界面方法
            updateBoardDel += new updateBoard(this.updateBoardMethod);
        }

        /// <summary>
        /// 打开关闭串口方法
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void button2_Click(object sender, EventArgs e) {
            radar = Radar.getInstance(this.textBox1.Text.Trim());
            Radar.afterProcessDel = new Radar.afterProcess(this.processData);
            if (!radar.IsOpen) {
                radar.openPort();
                this.button2.Text = "closePort";
                this.textBox1.Enabled = false;
            }
            else {
                radar.closePort();
                this.button2.Text = "openPort";
            }
        }

        /// <summary>
        /// 处理完数据执行画图方法
        /// </summary>
        public void processData() {
            Image img = dImg.drawImg(radar.Dist, radar.Back);
            if (updateBoardDel != null) {
                this.Invoke(updateBoardDel,img);
            }
        }

        /// <summary>
        /// 更新窗口委托
        /// </summary>
        /// <param name="img"></param>
        public delegate void updateBoard(Image img);
        public updateBoard updateBoardDel;

        public void updateBoardMethod(Image img) {
            this.pictureBox1.Image = img;
            this.textSpeed.Text = radar.Speed.ToString();
        }

    }
}