大家好,我最近在做一个有关动作捕捉的小项目,要求是在手臂不同节点穿戴惯性传感器进行实时动态的动作捕捉,在Unity中绑定人体模型实现姿态复现。大体效果类似下图,图片来自文献[1]。
因为成本有限,而且没学过单片机和电路相关知识,于是在某宝上购买了维特智能蓝牙低功耗传感器,型号是BWT901BLECL5.0,产品资料可在维特智能官网搜到,这里贴个链接:http://wit-motion.cn/#/witmotion/search
这款传感器的通信协议如下,方便大家对照后面的代码进行参考。下面是产品说明书里的部分通信协议说明:
默认输出数据:
发送指令能得到的数据:
unity存在旋转方向的问题导致用默认的输出数据的角度无法准确复原姿态,所以我选取了发送指令接收四元数数据的方法,用传回来的四元数数据为驱动,四元数回传的数据格式如下:
上面算是对问题背景的大体介绍,在搜索了网上有关于BLE和Unity结合的资料后,参考这位博主的文章
《使用Unity开发在PC端连接并接收蓝牙数据》https://blog.csdn.net/qq_42419143/article/details/113605331
进行了初步调试,可以在界面选择需要的设备名称、服务码、特征码获取实时输出回传数据,我没有用到write功能,界面图片如下:
其中,服务码为0000ffe5开头的那个,特征码一共两个,0000ffe4的properties是notify,选取后自动输出默认回传数据,0000ffe9的是write和write_no_response,用于向传感器发送指令
借助BLEWinrtdll和上面的文章代码,完成了单节点的姿态复现,代码如下:
using System;
using System.Collections;
using System.Collections.Generic;
using System.Text;
using UnityEngine;
using UnityEngine.EventSystems;
using UnityEngine.UI;
public class Demo : MonoBehaviour
{
public bool isScanningDevices = false;
public bool isScanningServices = false;
public bool isScanningCharacteristics = false;
public bool isSubscribed = false;
public Text deviceScanButtonText;
public Text deviceScanStatusText;
public GameObject deviceScanResultProto;
public Button serviceScanButton;
public Text serviceScanStatusText;
public Dropdown serviceDropdown;
public Button characteristicScanButton;
public Text characteristicScanStatusText;
public Dropdown characteristicDropdown;
public Button subscribeButton;
public Text subcribeText;
public Button writeButton;
public InputField writeInput;
public Text errorText;
Transform scanResultRoot;
public string selectedDeviceId;
public string selectedServiceId;
Dictionary<string, string> characteristicNames = new Dictionary<string, string>();
public string selectedCharacteristicId;
Dictionary<string, Dictionary<string, string>> devices = new Dictionary<string, Dictionary<string, string>>();
string lastError;
//四元数相关数据
public float x;
public float y;
public float z;
public float w;
int messagelen = 20;
public Quaternion currentRotation = new Quaternion();
// Start is called before the first frame update
void Start()
{
scanResultRoot = deviceScanResultProto.transform.parent;
deviceScanResultProto.transform.SetParent(null);
}
// Update is called once per frame
void Update()
{
BleApi.ScanStatus status;
if (isScanningDevices)
{
BleApi.DeviceUpdate res = new BleApi.DeviceUpdate();
do
{
status = BleApi.PollDevice(ref res, false);
if (status == BleApi.ScanStatus.AVAILABLE)
{
if (!devices.ContainsKey(res.id))
devices[res.id] = new Dictionary<string, string>() {
{ "name", "" },
{ "isConnectable", "False" }
};
if (res.nameUpdated)
devices[res.id]["name"] = res.name;
if (res.isConnectableUpdated)
devices[res.id]["isConnectable"] = res.isConnectable.ToString();
// consider only devices which have a name and which are connectable
if (devices[res.id]["name"] != "" && devices[res.id]["isConnectable"] == "True")
{
// add new device to list
GameObject g = Instantiate(deviceScanResultProto, scanResultRoot);
g.name = res.id;
g.transform.GetChild(0).GetComponent<Text>().text = devices[res.id]["name"];
g.transform.GetChild(1).GetComponent<Text>().text = res.id;
}
}
else if (status == BleApi.ScanStatus.FINISHED)
{
isScanningDevices = false;
deviceScanButtonText.text = "Scan devices";
deviceScanStatusText.text = "finished";
}
} while (status == BleApi.ScanStatus.AVAILABLE);
}
if (isScanningServices)
{
BleApi.Service res = new BleApi.Service();
do
{
status = BleApi.PollService(out res, false);
//Debug.Log(res.uuid);
if (status == BleApi.ScanStatus.AVAILABLE)
{
serviceDropdown.AddOptions(new List<string> { res.uuid });
// first option gets selected
if (serviceDropdown.options.Count == 1)
SelectService(serviceDropdown.gameObject);
}
else if (status == BleApi.ScanStatus.FINISHED)
{
isScanningServices = false;
serviceScanButton.interactable = true;
serviceScanStatusText.text = "finished";
}
} while (status == BleApi.ScanStatus.AVAILABLE);
}
if (isScanningCharacteristics)
{
BleApi.Characteristic res = new BleApi.Characteristic();
do
{
status = BleApi.PollCharacteristic(out res, false);
//Debug.Log(res.uuid);
if (status == BleApi.ScanStatus.AVAILABLE)
{
string name = res.userDescription != "no description available" ? res.userDescription : res.uuid;
characteristicNames[name] = res.uuid;
characteristicDropdown.AddOptions(new List<string> { name });
// first option gets selected
if (characteristicDropdown.options.Count == 1)
SelectCharacteristic(characteristicDropdown.gameObject);
}
else if (status == BleApi.ScanStatus.FINISHED)
{
isScanningCharacteristics = false;
characteristicScanButton.interactable = true;
characteristicScanStatusText.text = "finished";
}
} while (status == BleApi.ScanStatus.AVAILABLE);
}
if (isSubscribed)
{
BleApi.BLEData res = new BleApi.BLEData();
while (BleApi.PollData(out res, false))
{
//向传感器发送四元数回传指令
byte[] payload = new byte[] { 255, 170, 39, 81, 0 };
BleApi.BLEData data = new BleApi.BLEData();
data.buf = new byte[512];
data.size = (short)payload.Length;
data.deviceId = selectedDeviceId;
data.serviceUuid = selectedServiceId;
data.characteristicUuid = "{0000ffe9-0000-1000-8000-00805f9a34fb}";
for (int i = 0; i < payload.Length; i++)
data.buf[i] = payload[i];
// no error code available in non-blocking mode
BleApi.SendData(in data, false);
subcribeText.text = BitConverter.ToString(res.buf, 0, res.size);
//数据包标志位判断
if (res.buf[0] == 85 && res.buf[1] == 113)
{
//如果小于则说明数据区尚未接收完整,
if (res.size < messagelen)
{
//跳出接收函数后之后继续处理数据
break;
}
x = (float)(short)(res.buf[5] << 8 | res.buf[4]) / 32768.0f;
y = (float)(short)(res.buf[7] << 8 | res.buf[6]) / 32768.0f;
z = (float)(short)(res.buf[9] << 8 | res.buf[8]) / 32768.0f;
w = (float)(short)(res.buf[11] << 8 | res.buf[10]) / 32768.0f;
currentRotation.Set(x, y, z, w);
GameObject cube = GameObject.Find("Cube");
cube.transform.rotation = currentRotation;
}
}
}
{
// log potential errors
BleApi.ErrorMessage res = new BleApi.ErrorMessage();
BleApi.GetError(out res);
if (lastError != res.msg)
{
Debug.LogError(res.msg);
errorText.text = res.msg;
lastError = res.msg;
}
}
}
private void OnApplicationQuit()
{
BleApi.Quit();
}
public void StartStopDeviceScan()
{
if (!isScanningDevices)
{
// start new scan
for (int i = scanResultRoot.childCount - 1; i >= 0; i--)
Destroy(scanResultRoot.GetChild(i).gameObject);
BleApi.StartDeviceScan();
isScanningDevices = true;
deviceScanButtonText.text = "Stop scan";
deviceScanStatusText.text = "scanning";
}
else
{
// stop scan
isScanningDevices = false;
BleApi.StopDeviceScan();
deviceScanButtonText.text = "Start scan";
deviceScanStatusText.text = "stopped";
}
}
public void SelectDevice(GameObject data)
{
for (int i = 0; i < scanResultRoot.transform.childCount; i++)
{
var child = scanResultRoot.transform.GetChild(i).gameObject;
child.transform.GetChild(0).GetComponent<Text>().color = child == data ? Color.red :
deviceScanResultProto.transform.GetChild(0).GetComponent<Text>().color;
}
selectedDeviceId = data.name;
serviceScanButton.interactable = true;
}
public void StartServiceScan()
{
if (!isScanningServices)
{
// start new scan
serviceDropdown.ClearOptions();
BleApi.ScanServices(selectedDeviceId);
isScanningServices = true;
serviceScanStatusText.text = "scanning";
serviceScanButton.interactable = false;
}
}
public void SelectService(GameObject data)
{
selectedServiceId = serviceDropdown.options[serviceDropdown.value].text;
characteristicScanButton.interactable = true;
}
public void StartCharacteristicScan()
{
if (!isScanningCharacteristics)
{
// start new scan
characteristicDropdown.ClearOptions();
BleApi.ScanCharacteristics(selectedDeviceId, selectedServiceId);
isScanningCharacteristics = true;
characteristicScanStatusText.text = "scanning";
characteristicScanButton.interactable = false;
}
}
public void SelectCharacteristic(GameObject data)
{
string name = characteristicDropdown.options[characteristicDropdown.value].text;
selectedCharacteristicId = characteristicNames[name];
subscribeButton.interactable = true;
writeButton.interactable = true;
}
public void Subscribe()
{
// no error code available in non-blocking mode
BleApi.SubscribeCharacteristic(selectedDeviceId, selectedServiceId, selectedCharacteristicId, false);
isSubscribed = true;
}
}
在文章博主的指点下,我尝试在subscribe前区分多个节点,然后通过一次订阅,同时输出多节点数据,但是我并不清楚subscribecharacteristic函数能否在不同参数条件下同时运行多个,所以我对代码内容进行了修改,尝试同时完成两个传感器的姿态复现
using System;
using System.Collections;
using System.Collections.Generic;
using System.Text;
using UnityEngine;
using UnityEngine.EventSystems;
using UnityEngine.UI;
public class Demo : MonoBehaviour
{
public bool isScanningDevices = false;
public bool isScanningServices = false;
public bool isScanningCharacteristics = false;
public Text deviceScanButtonText;
public Text deviceScanStatusText;
public GameObject deviceScanResultProto;
public Button serviceScanButton;
public Text serviceScanStatusText;
public Dropdown serviceDropdown;
public Button characteristicScanButton;
public Text characteristicScanStatusText;
public Dropdown characteristicDropdown;
public Button subscribeButton;
public Text subcribeText;
public Button writeButton;
public InputField writeInput;
public Text errorText;
Transform scanResultRoot;
public string selectedDeviceId;
public string selectedServiceId;
Dictionary<string, string> characteristicNames = new Dictionary<string, string>();
public string selectedCharacteristicId;
Dictionary<string, Dictionary<string, string>> devices = new Dictionary<string, Dictionary<string, string>>();
string lastError;
public float x1;
public float y1;
public float z1;
public float w1;
int messagelen = 20;
public Quaternion currentRotation1 = new Quaternion();
public float x2;
public float y2;
public float z2;
public float w2;
public Quaternion currentRotation2 = new Quaternion();
public List<string> deviceIdd = new List<string>();
public int k = 0;
public List<bool> isSubscribed = new List<bool>();
// Start is called before the first frame update
void Start()
{
scanResultRoot = deviceScanResultProto.transform.parent;
deviceScanResultProto.transform.SetParent(null);
deviceIdd.Add("BluetoothLE#BluetoothLEec:5c:68:64:6b:52-f9:54:e1:60:a1:55");
deviceIdd.Add("BluetoothLE#BluetoothLEec:5c:68:64:6b:52-d4:da:58:f8:ab:83");
deviceIdd.Add("BluetoothLE#BluetoothLEec:5c:68:64:6b:52-d0:3e:7d:a4:1f:b7");
isSubscribed.Add(false);
isSubscribed.Add(false);
isSubscribed.Add(false);
//deviceIdd[3] = "BluetoothLE#BluetoothLEec:5c:68:64:6b:52-d0:3e:7d:a4:ca:cd";
Debug.Log(deviceIdd[0] + deviceIdd[1] + deviceIdd[2]);
//deviceIdd[4] = "BluetoothLE#BluetoothLEec:5c:68:64:6b:52-d0:3e:7d:a4:7a:7b";
// deviceIdd[5] = "BluetoothLE#BluetoothLEec:5c:68:64:6b:52-d0:3e:7d:a4:bb:f0";
// deviceIdd[6] = "BluetoothLE#BluetoothLEec:5c:68:64:6b:52-d0:3e:7d:a4:b3:38";
}
// Update is called once per frame
void Update()
{
BleApi.ScanStatus status;
if (isScanningDevices)
{
BleApi.DeviceUpdate res = new BleApi.DeviceUpdate();
do
{
status = BleApi.PollDevice(ref res, false);
if (status == BleApi.ScanStatus.AVAILABLE)
{
if (!devices.ContainsKey(res.id))
devices[res.id] = new Dictionary<string, string>() {
{ "name", "" },
{ "isConnectable", "False" }
};
if (res.nameUpdated)
devices[res.id]["name"] = res.name;
if (res.isConnectableUpdated)
devices[res.id]["isConnectable"] = res.isConnectable.ToString();
// consider only devices which have a name and which are connectable
if (devices[res.id]["name"] != "" && devices[res.id]["isConnectable"] == "True")
{
// add new device to list
GameObject g = Instantiate(deviceScanResultProto, scanResultRoot);
g.name = res.id;
g.transform.GetChild(0).GetComponent<Text>().text = devices[res.id]["name"];
g.transform.GetChild(1).GetComponent<Text>().text = res.id;
}
}
else if (status == BleApi.ScanStatus.FINISHED)
{
isScanningDevices = false;
deviceScanButtonText.text = "Scan devices";
deviceScanStatusText.text = "finished";
}
} while (status == BleApi.ScanStatus.AVAILABLE);
}
if (isScanningServices)
{
BleApi.Service res = new BleApi.Service();
do
{
status = BleApi.PollService(out res, false);
//Debug.Log(res.uuid);
if (status == BleApi.ScanStatus.AVAILABLE)
{
serviceDropdown.AddOptions(new List<string> { res.uuid });
// first option gets selected
if (serviceDropdown.options.Count == 1)
SelectService(serviceDropdown.gameObject);
}
else if (status == BleApi.ScanStatus.FINISHED)
{
isScanningServices = false;
serviceScanButton.interactable = true;
serviceScanStatusText.text = "finished";
}
} while (status == BleApi.ScanStatus.AVAILABLE);
}
if (isScanningCharacteristics)
{
BleApi.Characteristic res = new BleApi.Characteristic();
do
{
status = BleApi.PollCharacteristic(out res, false);
//Debug.Log(res.uuid);
if (status == BleApi.ScanStatus.AVAILABLE)
{
string name = res.userDescription != "no description available" ? res.userDescription : res.uuid;
characteristicNames[name] = res.uuid;
characteristicDropdown.AddOptions(new List<string> { name });
// first option gets selected
if (characteristicDropdown.options.Count == 1)
SelectCharacteristic(characteristicDropdown.gameObject);
}
else if (status == BleApi.ScanStatus.FINISHED)
{
isScanningCharacteristics = false;
characteristicScanButton.interactable = true;
characteristicScanStatusText.text = "finished";
}
} while (status == BleApi.ScanStatus.AVAILABLE);
}
if (isSubscribed[0])
{
BleApi.BLEData res1 = new BleApi.BLEData();
while (BleApi.PollData(out res1, false))
{
//对传感器1发送四元数指令数据
byte[] payload1 = new byte[] { 255, 170, 39, 81, 0 };
BleApi.BLEData data1 = new BleApi.BLEData();
data1.buf = new byte[512];
data1.size = (short)payload1.Length;
data1.deviceId = deviceIdd[0];
//data.serviceUuid = selectedServiceId;
data1.serviceUuid = "{0000ffe5-0000-1000-8000-00805f9a34fb}";
data1.characteristicUuid = "{0000ffe9-0000-1000-8000-00805f9a34fb}";
for (int i = 0; i < payload1.Length; i++)
data1.buf[i] = payload1[i];
// no error code available in non-blocking mode
BleApi.SendData(in data1, false);
subcribeText.text = BitConverter.ToString(res1.buf, 0, res1.size);
// subcribeText.text = Encoding.ASCII.GetString(res.buf, 0, res.size);
if (res1.buf[0] == 85 && res1.buf[1] == 113)
{
//如果小于则说明数据区尚未接收完整,
if (res1.size < messagelen)
{
//跳出接收函数后之后继续处理数据
break;
}
x1 = (float)(short)(res1.buf[5] << 8 | res1.buf[4]) / 32768.0f;
y1 = (float)(short)(res1.buf[7] << 8 | res1.buf[6]) / 32768.0f;
z1 = (float)(short)(res1.buf[9] << 8 | res1.buf[8]) / 32768.0f;
w1 = (float)(short)(res1.buf[11] << 8 | res1.buf[10]) / 32768.0f;
currentRotation1.Set(x1, y1, z1, w1);
GameObject cube1 = GameObject.Find("Cube");
cube1.transform.rotation = currentRotation1;
}
}
}
if (isSubscribed[1])
{
BleApi.BLEData res2 = new BleApi.BLEData();
while (BleApi.PollData(out res2, false))
{
//对传感器2发送四元数指令数据
byte[] payload2 = new byte[] { 255, 170, 39, 81, 0 };
BleApi.BLEData data2 = new BleApi.BLEData();
data2.buf = new byte[512];
data2.size = (short)payload2.Length;
data2.deviceId = deviceIdd[1];
//data.serviceUuid = selectedServiceId;
data2.serviceUuid = "{0000ffe5-0000-1000-8000-00805f9a34fb}";
data2.characteristicUuid = "{0000ffe9-0000-1000-8000-00805f9a34fb}";
for (int i = 0; i < payload2.Length; i++)
data2.buf[i] = payload2[i];
// no error code available in non-blocking mode
BleApi.SendData(in data2, false);
subcribeText.text = BitConverter.ToString(res2.buf, 0, res2.size);
// subcribeText.text = Encoding.ASCII.GetString(res.buf, 0, res.size);
if (res2.buf[0] == 85 && res2.buf[1] == 113)
{
//如果小于则说明数据区尚未接收完整,
if (res2.size < messagelen)
{
//跳出接收函数后之后继续处理数据
break;
}
x2 = (float)(short)(res2.buf[5] << 8 | res2.buf[4]) / 32768.0f;
y2 = (float)(short)(res2.buf[7] << 8 | res2.buf[6]) / 32768.0f;
z2 = (float)(short)(res2.buf[9] << 8 | res2.buf[8]) / 32768.0f;
w2 = (float)(short)(res2.buf[11] << 8 | res2.buf[10]) / 32768.0f;
currentRotation2.Set(x2, y2, z2, w2);
GameObject cube2 = GameObject.Find("Cubee");
cube2.transform.rotation = currentRotation2;
}
}
}
{
// log potential errors
BleApi.ErrorMessage res = new BleApi.ErrorMessage();
BleApi.GetError(out res);
if (lastError != res.msg)
{
Debug.LogError(res.msg);
errorText.text = res.msg;
lastError = res.msg;
}
}
}
private void OnApplicationQuit()
{
BleApi.Quit();
}
public void StartStopDeviceScan()
{
if (!isScanningDevices)
{
// start new scan
for (int i = scanResultRoot.childCount - 1; i >= 0; i--)
Destroy(scanResultRoot.GetChild(i).gameObject);
BleApi.StartDeviceScan();
isScanningDevices = true;
deviceScanButtonText.text = "Stop scan";
deviceScanStatusText.text = "scanning";
}
else
{
// stop scan
isScanningDevices = false;
BleApi.StopDeviceScan();
deviceScanButtonText.text = "Start scan";
deviceScanStatusText.text = "stopped";
}
}
public void SelectDevice(GameObject data)
{
for (int i = 0; i < scanResultRoot.transform.childCount; i++)
{
var child = scanResultRoot.transform.GetChild(i).gameObject;
child.transform.GetChild(0).GetComponent<Text>().color = child == data ? Color.red :
deviceScanResultProto.transform.GetChild(0).GetComponent<Text>().color;
}
selectedDeviceId = data.name;
serviceScanButton.interactable = true;
}
public void StartServiceScan()
{
if (!isScanningServices)
{
// start new scan
serviceDropdown.ClearOptions();
BleApi.ScanServices(selectedDeviceId);
isScanningServices = true;
serviceScanStatusText.text = "scanning";
serviceScanButton.interactable = false;
}
}
public void SelectService(GameObject data)
{
selectedServiceId = serviceDropdown.options[serviceDropdown.value].text;
characteristicScanButton.interactable = true;
}
public void StartCharacteristicScan()
{
if (!isScanningCharacteristics)
{
// start new scan
characteristicDropdown.ClearOptions();
BleApi.ScanCharacteristics(selectedDeviceId, selectedServiceId);
isScanningCharacteristics = true;
characteristicScanStatusText.text = "scanning";
characteristicScanButton.interactable = false;
}
}
public void SelectCharacteristic(GameObject data)
{
string name = characteristicDropdown.options[characteristicDropdown.value].text;
selectedCharacteristicId = characteristicNames[name];
subscribeButton.interactable = true;
writeButton.interactable = true;
}
public void Subscribe()
{
// no error code available in non-blocking mode
for(k = 0; k < 2; k++)
{
BleApi.SubscribeCharacteristic(deviceIdd[k], "{0000ffe5-0000-1000-8000-00805f9a34fb}", "{0000ffe4-0000-1000-8000-00805f9a34fb}", false);
isSubscribed[k] = true;
Debug.Log("isSubscribed[" + k + "]" + isSubscribed[k]);
//此时k=2
}
}
public void Write()
{
byte[] payload = Encoding.ASCII.GetBytes(writeInput.text);
BleApi.BLEData data = new BleApi.BLEData();
data.buf = new byte[512];
data.size = (short)payload.Length;
data.deviceId = selectedDeviceId;
data.serviceUuid = selectedServiceId;
data.characteristicUuid = selectedCharacteristicId;
for (int i = 0; i < payload.Length; i++)
data.buf[i] = payload[i];
// no error code available in non-blocking mode
BleApi.SendData(in data, false);
}
}
按我的想法,两个传感器都可以分别输出对应的数据并完成姿态复现,然而只有第一个传感器可以匹配cube进行复现,第二个传感器匹配的cubee没有按现实世界的传感器的姿态变化而实时变化
我不知道是哪里出了问题,是因为subscribecharacteristic无法同时在多个传感器上运行吗,或者是代码的逻辑本身就有问题,因为我没有系统地学过c#和unity的知识,目前的成果都是在网上搜索材料后照猫画虎完成的。劳烦有相关经验的同行帮忙分析、解答。
参考文献:
[1]Wang B, Zhou H, Yang G, et al. Human Digital Twin (HDT) Driven Human-Cyber-Physical Systems: Key Technologies and Applications[J]. Chinese Journal of Mechanical Engineering, 2022, 35(1): 1-6.