业务需求用到了G4激光雷达,需要对接雷达的sdk,最方便的是直接找unitypackage包来用,网上要么收费要么没用。所以还是自己对接吧
准备
先找相关文档,官网,sdk样例等。官网里各种文档和sdk都有。
Git
官网
For C#
1.首先需要有一个g4雷达(注意雷达型号,不同型号可能一些初始化参数不一样 例如串口波特率等)
2.根据使用文档,还要下载安装串口驱动
3.打开官网下载的雷达查看程序,选好雷达usb串口。就能在软件里看到雷达的运行了。
SKD
要自己使用雷达,就得使用它的sdk
1.下载雷达sdk包,下载完惊不惊喜意不意外 你还得下个cmake去给他弄成vs工程能打开的。
2.然后发现它虽然文档里写的有python,C# 结果sdk里就写了个C#脚本去调用。要在unity用你还得自己打链接库。
3.本着不自己造轮子的原则(其实是自己打dll老报错。。。)还真让我在github上找到了sdk的64位dll
4.将dll拉入unity的plugins中,开始调用!
这里不要忽视了下载的sdk程序,里边有个console启动案例,你启动运行一下,并结合它案例的cpp代码,就知道在调用雷达后,它传递给你的数据格式。
数据格式
这里我就直接发出来了,(以检测设置为180度来说)就是 给你发送一个数组,这个数组记录了0—180度的所有点(离雷达的距离 角度)
官方话来说就是 点云 数据
其实应该是有相关的算法来搞的,但我也不了解,只能自己来 对数据做分析,提取自己要的。
数据分析
我们可以知道360内的点距雷达的距离
- 将雷达的点云数据 划分出一个个线条轮廓 (相邻角度的点数据 相距距离判断)
- 对轮廓去除误差 (轮廓长度短的)
这样就能得到 一段一段的有效遮挡 取每一段的特征点的距离和角度 即可获得一个雷达上我们需要的点数据
其他的是取最近 取最远还是怎样根据业务需求来即可。
在不同距离下的误差值是变动的
代码:
- 初始化sdk里的雷达类(端口,波特率,扫描角度等参数)
- 调用sdk雷达类方法,启动雷达
- 开启线程获取并更新雷达数据
- 在协程里对数据做计算(不一定非要协程,注意线程不能用unity方法即可)
- 雷达sdk提供了点的角度和距离,根据这些计算有效数据
- 使用计算出的有效数据即可
找不到引用的基本上都是项目的业务相关
using System;
using System.Collections;
using System.Collections.Generic;
using System.Threading;
using UnityEngine;
using UnityEngine.SceneManagement;
using UnityEngine.UI;
using YDLidarSharp;
public class LeidaSDKFunction : MonoBehaviour
{
public Transform parent;
public GameObject prefab;
public GameObject people;
// Start is called before the first frame update
List<RectTransform> list = new List<RectTransform>();
YDLidarSDK sdk;
Thread getdata;
LaserScan ls;//扫描结果结构
const int Maxl = 1000;
double[] angle_ = new double[Maxl];
double[] ranger_ = new double[Maxl];
//初始化雷达相关参数
public void InitLeiDa()
{
if (sdk == null)
{
sdk = new YDLidarSDK();
}
//初始化sdk相关参数
SetSDKProperties();
//初始化计算数据相关阈值
SetCalculateData();
}
//开启雷达
public void StartLeiDa()
{
if (sdk.Initialize())
{
sdk.TurnOn();//开启雷达
//开启线程不断更新 雷达数据
getdata = new Thread(() => Getdatadd());
getdata.Start();
// 线程无法调用unity函数,只在线程更新数据,分析数据放在协程里
StartCoroutine(SetUIData());
}
else
{
Debug.LogError("雷达开启失败!");
}
}
public void Getdatadd()
{
while (true)
{
ls = sdk.GetData();
//北阳数据长度是固定的角度分辨率,G4数据长度不固定
//根据之前设置的雷达参数 打印下 调整合适的有效长度
if (ls.LaserPoints == null || ls.LaserPoints.Count < 500)
continue;
//sdk提供的扫描到的点数据:角度,距离
//根据角度从0-180排列
ls.LaserPoints.Sort((a, b) => a.Angle.CompareTo(b.Angle));//0-180
for (int i = 0; i < Maxl && i < ls.LaserPoints.Count; i++)//数据存储 保证Maxl>count
{
angle_[i] = ls.LaserPoints[i].Angle;
ranger_[i] = ls.LaserPoints[i].Range;
}
Thread.Sleep(1);
}
}
//关闭雷达
public void CloseLeiDa()
{
if(getdata!=null)
getdata.Abort();
StopAllCoroutines();
sdk.TurnOff();
}
private void SetSDKProperties()
{
//雷达串口
sdk.SerialPort = SystemParam.LeiDaCom;
int baudrate;
int.TryParse(SystemParam.LeiDaBaudrate, out baudrate);
sdk.SerialBaudrate = baudrate;//雷达波特率
//不管
sdk.FixedResolution = false;
sdk.Reversion = true;
sdk.AutoReconnect = true;
sdk.SampleRate = 9;//?
//雷达扫描角度0-180
sdk.MaxAngle = 180;
sdk.MinAngle = 0;
//雷达扫描距离 0.15m-6m
sdk.MinRange = 0.15f;
sdk.MaxRange = 6;
int fre;
int.TryParse(SystemParam.LeiDaBaudrate, out fre);
//扫描频率 貌似对扫描到的数据长度有影响
sdk.ScanFrequency = fre;
}
private void SetCalculateData()
{
//数据提取距离
float tempfloat;
int tempint;
float.TryParse(SystemParam.LeiDaMinDataRange, out tempfloat);
LeiDaPoint.minR = minR = tempfloat;
float.TryParse(SystemParam.LeiDaMaxDataRange, out tempfloat);
LeiDaPoint.maxR = maxR = tempfloat;
float.TryParse(SystemParam.MoveSensitivity, out tempfloat);
LeiDaPoint.MoveSensitivity = tempfloat;
//将点数据分段的有效长度
float.TryParse(SystemParam.LeiDaEndLineLength, out tempfloat);
lineEndLenght = tempfloat;
int.TryParse(SystemParam.LeiDaLineMinAngle, out tempint);
lineMinAngle = tempint;
}
//UI模拟雷达点
public void CreateUIMap()
{
anglescale = 180f / Maxl;
for (int i = 0; i < Maxl; i++)
{
Vector2 t;
t.x = Mathf.Cos(i * anglescale * Mathf.PI / 180) * 200;
t.y = Mathf.Sin(i * anglescale * Mathf.PI / 180) * 200;
RectTransform temp = Instantiate(prefab, parent).GetComponent<RectTransform>();
temp.anchoredPosition = t;
list.Add(temp);
}
showUIPoint = true;
}
int scale = 1000 / 2;
float minR = 0.3f;
float maxR = 1f;
Dictionary<string, List<double>> keyValuePairs = new Dictionary<string, List<double>>();
Dictionary<int, List<double>> finalLine = new Dictionary<int, List<double>>();
List<LeiDaPoint> getDatas = new List<LeiDaPoint>();
bool isSort = false;
bool showUIPoint = false;
float anglescale = 0;
float lineEndLenght = 0.1f;
int lineMinAngle = 5;
/// <summary>
/// 解析雷达数据
/// 解析思路:
///
/// 将雷达的点云数据 划分出一个个线条轮廓 (相邻角度的点数据 相距距离判断)
/// 对轮廓去除误差 (轮廓长度短的)
///
///
/// </summary>
/// <returns></returns>
IEnumerator SetUIData()
{
while (true)
{
yield return null;
if (ls.LaserPoints == null || ls.LaserPoints.Count < 500)
continue;
//根据之前设置的角度范围来 计算角度分辨率
anglescale = 180f / ls.LaserPoints.Count;
LeiDaPoint.anglescale = anglescale;//项目业务数据 忽略
Vector2 startPos = Vector2.zero;
//double startRange = 0;
string x = string.Empty;
keyValuePairs.Clear();
//遍历点云数据
for (int i = 0; i < Maxl && i < ls.LaserPoints.Count; i++)
{
//是否需要ui模拟点云分布
if (showUIPoint)
{
#region ui模拟排布
//if (isSort)
// if (ranger_[i] > 1)
// {
// list[i].anchoredPosition = Vector2.zero;
// }
//ui坐标计算
Vector2 t;
t.x = Mathf.Cos(i * anglescale * Mathf.PI / 180) * (float)ranger_[i] * scale;
t.y = Mathf.Sin(i * anglescale * Mathf.PI / 180) * (float)ranger_[i] * scale;
list[i].anchoredPosition = t;
list[i].localScale = Vector3.one * 0.072853f;
#endregion
}
#region 点位计算
//位置点计算(计算指定范围的点) //业务需要的最近最远距离内的数据
if (ranger_[i] > minR && ranger_[i] < maxR)
{
//将点转换到坐标 进行比较
Vector2 nowPos;
nowPos.x = Mathf.Cos(i * anglescale * Mathf.PI / 180) * (float)ranger_[i];
nowPos.y = Mathf.Sin(i * anglescale * Mathf.PI / 180) * (float)ranger_[i];
if (string.IsNullOrEmpty(x))//起始标志 第一个记录点
{
//
startPos = nowPos;
//startRange = ranger_[i];
x = i.ToString();
keyValuePairs.Add(x, new List<double>() { ranger_[i] });
}
else//和起始点比较
{
//小于阈值,判定为一条线段内的数据点
//待优化:随着与雷达的距离远近,阈值应该是动态的,即越近阈值可以越小,越远阈值越大
if (Vector2.Distance(nowPos, startPos) < lineEndLenght)//两点距离阈值 range越大 阈值越大
{
keyValuePairs[x].Add(ranger_[i]);
startPos = nowPos;
//startRange = ranger_[i];
}
else//断开当前线条
{
//待优化:给定容错
//中断
x = string.Empty;
startPos = Vector2.zero;
}
}
}
#endregion
}
//遍历一遍后获取到的keyValuePairs线段
finalLine.Clear();
foreach (var va in keyValuePairs.Keys)
{
int pp;
int.TryParse(va, out pp);
//#region 原始人物点位UI模拟
//list[pp].localScale = Vector3.one * 0.072853f * 5;
//#endregion
#region 点位去杂(去除长度不够的点)
if (keyValuePairs[va].Count > lineMinAngle / anglescale)//占5度
{
finalLine.Add(pp, keyValuePairs[va]);
}
#endregion
}
getDatas.Clear();
//去除无效数据后的线条数据
//将首个位置作为 线段(人) 位置
foreach (var N in finalLine.Keys)
{
//int value = N + finalLine[N].Count / 2;
int value = N;
#region 点位UI模拟 (中值会有波动,使用首个位置)
if(showUIPoint)
list[value].localScale = Vector3.one * 0.072853f * 5;
#endregion
//range首个位置的距离
getDatas.Add(new LeiDaPoint(value, finalLine[N][0]));
}
//数据由近到远排列
//getDatas.Sort((a, b) => a.range.CompareTo(b.range));
//修改成由距屏幕近到远
getDatas.Sort((a, b) => a.pos.y.CompareTo(b.pos.y));
// Debug.Log(getDatas[0].range);
//发送获取到的数据点
if (EventManager.ins != null)
{
EventManager.ins.DispatchEvent(Showroom.EventType.SendPonitData, getDatas);
}
}
}
bool op = false;
// Update is called once per frame
void Update()
{
//if (Input.GetKey(KeyCode.Alpha1))
//{
// isSort = !isSort;
//}
if (Input.GetKey(KeyCode.Alpha2))
{
if (!op)
{
CreateUIMap();
op = true;
}
}
//if (Input.GetKey(KeyCode.K))
//{
// scale = 1000;
//}
//if (Input.GetAxis("Mouse ScrollWheel") > 0)
//{
// scale -= 10;
//}
//if (Input.GetAxis("Mouse ScrollWheel") < 0)
//{
// scale += 10;
//}
}
private void OnDestroy()
{
CloseLeiDa();
sdk.Disconnecting();
}
}
补充:
后续又用到北阳雷达等,其实是一样的
北阳雷达:
通过它自己带的程序发送雷达数据。自己写socket监听数据发送。
北阳比g4好的是,它一种设备是固定扫描角度频率,从设备参数就可以了解:
【测量距离:0.06 to 10m, Max.30m, 270°】
【角度分辨率=0.25° (360°/1,440 steps)】
即它的数据给的是270/0.25份。而不是像g4一样不太精确