/// <summary>
/// Crates a new <see cref="BaseStringGlarduinoClient"/>.
/// </summary>
/// <param name="connectionInfo">The connection information for the port.</param>
/// <param name="messageDeserializer">The message deserialization strategy.</param>
/// <param name="messageDispatcher">The message dispatching strategy.</param>
/// <param name="comPort">The specified communication port.</param>
public BaseStringGlarduinoClient(ArduinoPortConnectionInfo connectionInfo,
IMessageDeserializerStrategy <string> messageDeserializer,
IMessageDispatchingStrategy <string> messageDispatcher,
ICommunicationPort comPort)
: base(connectionInfo, messageDeserializer, messageDispatcher, comPort)
{
}
/// <inheritdoc />
protected BaseUnityGlarduinoClient(ArduinoPortConnectionInfo connectionInfo,
IMessageDeserializerStrategy <TMessageType> messageDeserializer,
IMessageDispatchingStrategy <TMessageType> messageDispatcher,
ICommunicationPort comPort)
: base(connectionInfo, messageDeserializer, messageDispatcher, comPort)
{
}
/// <summary>
/// Reads a chunk into the provided <paramref name="buffer"/> from the provided <see cref="SerialPort"/>.
/// </summary>
/// <param name="serialPort">The port to read from.</param>
/// <param name="buffer">The buffer to read into.</param>
/// <param name="offset">The offset into the buffer.</param>
/// <param name="count">The count of bytes to read.</param>
/// <param name="cancellationToken">Optional cancellation token for the read operation.</param>
/// <returns>Awaitable for when the operation is completed.</returns>
public static async Task ReadAsync(this ICommunicationPort serialPort, byte[] buffer, int offset, int count, CancellationToken cancellationToken = default(CancellationToken))
{
if (serialPort == null)
{
throw new ArgumentNullException(nameof(serialPort));
}
if (buffer == null)
{
throw new ArgumentNullException(nameof(buffer));
}
if (offset < 0)
{
throw new ArgumentOutOfRangeException(nameof(offset));
}
if (count < 0)
{
throw new ArgumentOutOfRangeException(nameof(count));
}
if (!serialPort.IsOpen)
{
throw new InvalidOperationException($"Provided {nameof(serialPort)} is not in an open state. Cannot read.");
}
for (int i = 0; i < count && !cancellationToken.IsCancellationRequested;)
{
int readBytes = await serialPort.BaseStream.ReadAsync(buffer, i + offset, count - i, cancellationToken)
.ConfigureAwait(false);
i += readBytes;
}
}
/// <inheritdoc />
public UnityQuaternionSegmentGlarduinoClient(ArduinoPortConnectionInfo connectionInfo,
IMessageDeserializerStrategy <RecyclableArraySegment <Quaternion> > messageDeserializer,
IMessageDispatchingStrategy <RecyclableArraySegment <Quaternion> > messageDispatcher,
ICommunicationPort comPort)
: base(connectionInfo, messageDeserializer, messageDispatcher, comPort)
{
}
/// <summary>
/// Gets the size of the incoming <see cref="Quaternion"/> array.
/// </summary>
/// <param name="serialPort">The communication port.</param>
/// <param name="cancellationToken">Optional cancel token.</param>
/// <returns>Awaitable byte future.</returns>
protected virtual async Task <byte> GetQuaternionCountAsync(ICommunicationPort serialPort, CancellationToken cancellationToken)
{
await serialPort.ReadAsync(SingleByteBuffer, 0, 1, cancellationToken);
byte quaternionCount = SingleByteBuffer[0];
return(quaternionCount);
}
public Controller(ICommunicationPort communicationPort, IPersistedStorage persistedStorage)
{
CommunicationPort = communicationPort;
CommunicationPortLock = new object();
RefreshInterval = 150;
GlobalConfiguration = new GlobalConfiguration(this);
IgnitionMap = new IgnitionMap(this, persistedStorage);
CurrentState = new CurrentState(this);
}
/// <summary>
/// Crates a new <see cref="BaseGlarduinoClient{TMessageType}"/>.
/// Specifically defining the communication port.
/// </summary>
/// <param name="connectionInfo">The connection information for the port.</param>
/// <param name="messageDeserializer">The message deserialization strategy.</param>
/// <param name="messageDispatcher">The message dispatching strategy.</param>
/// <param name="comPort">The specified communication port.</param>
protected BaseGlarduinoClient(ArduinoPortConnectionInfo connectionInfo,
IMessageDeserializerStrategy <TMessageType> messageDeserializer,
IMessageDispatchingStrategy <TMessageType> messageDispatcher,
ICommunicationPort comPort)
{
ConnectionInfo = connectionInfo ?? throw new ArgumentNullException(nameof(connectionInfo));
MessageDeserializer = messageDeserializer ?? throw new ArgumentNullException(nameof(messageDeserializer));
MessageDispatcher = messageDispatcher ?? throw new ArgumentNullException(nameof(messageDispatcher));
InternallyManagedPort = comPort ?? throw new ArgumentNullException(nameof(comPort));
_ConnectionEvents = new ConnectionEvents();
}
/// <inheritdoc />
public async Task <RecyclableArraySegment <Quaternion> > ReadMessageAsync(ICommunicationPort serialPort, CancellationToken cancellationToken = default(CancellationToken))
{
byte quaternionCount = await GetQuaternionCountAsync(serialPort, cancellationToken);
if (quaternionCount == 0)
{
return(RecyclableArraySegment <Quaternion> .Empty);
}
//Now we should read the quats based on the provided count
var quatArray = ArrayPool <Quaternion> .Shared.Rent(quaternionCount);
try
{
for (int i = 0; i < quaternionCount && !cancellationToken.IsCancellationRequested; i++)
{
await serialPort.ReadAsync(SingleQuatBuffer, 0, SingleQuatBuffer.Length, cancellationToken);
//Just return whatever we have if cancelled.
if (cancellationToken.IsCancellationRequested)
{
return(new RecyclableArraySegment <Quaternion>(quatArray, 0, quaternionCount));
}
//quatArray[i] = new Quaternion(GetFloatFromQuatBuffer(1), GetFloatFromQuatBuffer(2), GetFloatFromQuatBuffer(3), GetFloatFromQuatBuffer(0));
quatArray[i] = new Quaternion(GetFloatFromQuatBuffer(0), GetFloatFromQuatBuffer(1), GetFloatFromQuatBuffer(2), GetFloatFromQuatBuffer(3));
if (GlardunioStaticConfig.DebugLogging)
{
Debug.Log($"Quat log: {quatArray[i].x} {quatArray[i].y} {quatArray[i].z} {quatArray[i].w}");
}
}
return(new RecyclableArraySegment <Quaternion>(quatArray, 0, quaternionCount));
}
catch (Exception e)
{
throw;
}
finally
{
//If exceptions happen let's dipose of the array
ArrayPool <Quaternion> .Shared.Return(quatArray);
}
}
/// <summary>
/// Read a line from the SerialPort asynchronously
/// </summary>
/// <param name="serialPort">The port to read data from</param>
/// <returns>A line read from the input</returns>
public static async Task <string> ReadLineAsync(this ICommunicationPort serialPort)
{
if (!serialPort.Encoding.IsSingleByte)
{
throw new InvalidOperationException($"TODO: Cannot support {serialPort.Encoding.EncodingName}. Only supports 1 byte ASCII right now.");
}
int charSizeCount = serialPort.Encoding.IsSingleByte ? 1 : serialPort.Encoding.GetMaxByteCount(1);
byte[] _singleCharBuffer = ArrayPool <byte> .Shared.Rent(charSizeCount);
StringBuilder builder = new StringBuilder();
try
{
// Read the input one byte at a time, convert the
// byte into a char, add that char to the overall
// response string, once the response string ends
// with the line ending then stop reading
while (true)
{
await serialPort.ReadAsync(_singleCharBuffer, 0, 1)
.ConfigureAwait(false);
builder.Append((char)_singleCharBuffer[0]);
if (builder[builder.Length - 1] == DefaultNewLine)
{
// Truncate the line ending
return(builder.ToString(0, builder.Length - 1));
}
}
}
catch (Exception e)
{
throw;
}
finally
{
ArrayPool <byte> .Shared.Return(_singleCharBuffer, true);
}
}
private void Form1_Load(object sender, EventArgs e)
{
_estimator = new CompositePoseEstimator[numberOfLink];
// フィルタ：速い動きに追従する設定です
var fs = 1 / 0.01;
var cutoff = 25.0;
var tc = 0.4;
for (int i = 0; i < numberOfLink; i++)
{
_estimator[i] = new CompositePoseEstimator();
_estimator[i].SetFilterCoeffs(CompositePoseEstimator.FilterName.AccLowPassFilter, FilterDisignerBiquad.LowPass(fs, cutoff, 1.0));
_estimator[i].SetFilterCoeffs(CompositePoseEstimator.FilterName.CompLowPassFilter, FilterDisignerBiquad.LowPass(fs, cutoff, 1.0));
_estimator[i].SetFilterCoeffs(CompositePoseEstimator.FilterName.GyroLowPassFilter, FilterDisignerBiquad.LowPass(fs, 45.0, 1.0));
_estimator[i].TimeConstantComposition = tc;
}
// 接続処理
var dialog = new PortOpenDialog();
if (dialog.ShowDialog() != System.Windows.Forms.DialogResult.OK)
{
return;
}
_port = CommunicationManager.CreateCommunicationPort(dialog.ConnectionNameType);
// データ更新イベントの登録
_port.MsgMeasurementReceived += new ReceivedMsgMeasurementHandler(_port_MsgMeasurementReceived);
dialog.Dispose();
_port.Open();
#region 形状、位置設定
// 骨盤
_linkPelvis = new Link()
{
LinkId = 0,
ParentLinkId = -1,
Position = new Vec4(),
Orientation = Quaternion.CreateFromAngle(0, 0, 0),
};
var _linkPelvisShape = new Shape()
{
Type = ShapeType.Pyramid,
Scale = new Vec4(30.7 / scale, 3 / scale, 10 / scale, 0 / scale),
Orientation = Quaternion.CreateFromAngle(0, -Math.PI / 2, 0),
Position = new Vec4(0, 0, 0, 0),
};
_allLinkes.Add(_linkPelvis);
_allShapes.Add(_linkPelvisShape);
// 左大腿
_linkLeftThigh = new Link()
{
LinkId = 1,
ParentLinkId = 0,
Position = new Vec4(15.4 / scale, 0 / scale, 0 / scale),
Orientation = Quaternion.CreateFromAngle(0, 0, 0),
};
var _linkLeftThighShape = new Shape()
{
Type = ShapeType.Pyramid,
Scale = new Vec4(-38.1 / scale, 5 / scale, 3 / scale, 0 / scale),
Orientation = Quaternion.CreateFromAngle(0, -Math.PI / 2, 0),
Position = new Vec4(0, 0, 0, 0),
};
_allLinkes.Add(_linkLeftThigh);
_allShapes.Add(_linkLeftThighShape);
// 左脛
_linkLeftShin = new Link()
{
LinkId = 2,
ParentLinkId = 1,
Position = new Vec4(0 / scale, 0 / scale, -38.1 / scale),
Orientation = Quaternion.CreateFromAngle(0, 0, 0),
};
var _linkLeftShinShape = new Shape()
{
Type = ShapeType.Pyramid,
Scale = new Vec4(-46.5 / scale, 5 / scale, 3 / scale, 0 / scale),
Orientation = Quaternion.CreateFromAngle(0, -Math.PI / 2, 0),
Position = new Vec4(0, 0, 0, 0),
};
_allLinkes.Add(_linkLeftShin);
_allShapes.Add(_linkLeftShinShape);
// 左足首
_linkLeftFoot = new Link()
{
LinkId = 3,
ParentLinkId = 2,
Position = new Vec4(0 / scale, 0 / scale, -46.5 / scale),
Orientation = Quaternion.CreateFromAngle(0, 0, 0),
};
var _linkLeftFootShape = new Shape()
{
Type = ShapeType.Pyramid,
Scale = new Vec4(16.5 / scale, 2 / scale, 2 / scale, 0 / scale),
Orientation = Quaternion.CreateFromAngle(0, Math.PI / 2, -Math.PI / 2),
Position = new Vec4(0, 0, 0, 0),
};
_allLinkes.Add(_linkLeftFoot);
_allShapes.Add(_linkLeftFootShape);
// 右大腿
_linkRightThigh = new Link()
{
LinkId = 4,
ParentLinkId = 0,
Position = new Vec4(-15.4 / scale, 0 / scale, 0 / scale),
Orientation = Quaternion.CreateFromAngle(0, 0, 0),
};
var _linkRightThighShape = new Shape()
{
Type = ShapeType.Pyramid,
Scale = new Vec4(-38.1 / scale, 5 / scale, 3 / scale, 0 / scale),
Orientation = Quaternion.CreateFromAngle(0, -Math.PI / 2, 0),
Position = new Vec4(0, 0, 0, 0),
};
_allLinkes.Add(_linkRightThigh);
_allShapes.Add(_linkRightThighShape);
// 右脛
_linkRightShin = new Link()
{
LinkId = 5,
ParentLinkId = 4,
Position = new Vec4(0 / scale, 0 / scale, -38.1 / scale),
Orientation = Quaternion.CreateFromAngle(0, 0, 0),
};
var _linkRightShinShape = new Shape()
{
Type = ShapeType.Pyramid,
Scale = new Vec4(-46.5 / scale, 5 / scale, 3 / scale, 0 / scale),
Orientation = Quaternion.CreateFromAngle(0, -Math.PI / 2, 0),
Position = new Vec4(0, 0, 0, 0),
};
_allLinkes.Add(_linkRightShin);
_allShapes.Add(_linkRightShinShape);
// 右足首
_linkRightFoot = new Link()
{
LinkId = 6,
ParentLinkId = 5,
Position = new Vec4(0 / scale, 0 / scale, -46.5 / scale),
Orientation = Quaternion.CreateFromAngle(0, 0, 0),
};
var _linkRightFootShape = new Shape()
{
Type = ShapeType.Pyramid,
Scale = new Vec4(16.5 / scale, 2 / scale, 2 / scale, 0 / scale),
Orientation = Quaternion.CreateFromAngle(0, Math.PI / 2, -Math.PI / 2),
Position = new Vec4(0, 0, 0, 0),
};
_allLinkes.Add(_linkRightFoot);
_allShapes.Add(_linkRightFootShape);
#endregion
// キネマティクスの初期化
_kvm = new KinematicsViewModel(new ForwardKinematicsSolver());
_kvm.Init(_allLinkes, _allShapes);
// 描画領域に追加
foreach (var s in _allShapes)
{
imuzDraw1.AddPyramidModel((float)s.Scale.x, (float)s.Scale.y, (float)s.Scale.z);
}
return;
}
private void Form1_Load(object sender, EventArgs e)
{
_estimator = new CompositePoseEstimator[numberOfLink];
// フィルタ：速い動きに追従する設定です
var fs = 1 / 0.01;
var cutoff = 25.0;
var tc = 0.4;
for (int i = 0; i < numberOfLink; i++) {
_estimator[i] = new CompositePoseEstimator();
_estimator[i].SetFilterCoeffs(CompositePoseEstimator.FilterName.AccLowPassFilter, FilterDisignerBiquad.LowPass(fs, cutoff, 1.0));
_estimator[i].SetFilterCoeffs(CompositePoseEstimator.FilterName.CompLowPassFilter, FilterDisignerBiquad.LowPass(fs, cutoff, 1.0));
_estimator[i].SetFilterCoeffs(CompositePoseEstimator.FilterName.GyroLowPassFilter, FilterDisignerBiquad.LowPass(fs, 45.0, 1.0));
_estimator[i].TimeConstantComposition = tc;
}
// 接続処理
var dialog = new PortOpenDialog();
if (dialog.ShowDialog() != System.Windows.Forms.DialogResult.OK) { return; }
_port = CommunicationManager.CreateCommunicationPort(dialog.ConnectionNameType);
// データ更新イベントの登録
_port.MsgMeasurementReceived += new ReceivedMsgMeasurementHandler(_port_MsgMeasurementReceived);
dialog.Dispose();
_port.Open();
#region 形状、位置設定
// 骨盤
_linkPelvis = new Link() {
LinkId = 0,
ParentLinkId = -1,
Position = new Vec4(),
Orientation = Quaternion.CreateFromAngle(0, 0, 0),
};
var _linkPelvisShape = new Shape() {
Type = ShapeType.Pyramid,
Scale = new Vec4(30.7 / scale, 3 / scale, 10 / scale, 0 / scale),
Orientation = Quaternion.CreateFromAngle(0, -Math.PI / 2, 0),
Position = new Vec4(0, 0, 0, 0),
};
_allLinkes.Add(_linkPelvis);
_allShapes.Add(_linkPelvisShape);
// 左大腿
_linkLeftThigh = new Link() {
LinkId = 1,
ParentLinkId = 0,
Position = new Vec4(15.4 / scale, 0 / scale, 0 / scale),
Orientation = Quaternion.CreateFromAngle(0, 0, 0),
};
var _linkLeftThighShape = new Shape() {
Type = ShapeType.Pyramid,
Scale = new Vec4(-38.1 / scale, 5 / scale, 3 / scale, 0 / scale),
Orientation = Quaternion.CreateFromAngle(0, -Math.PI / 2, 0),
Position = new Vec4(0, 0, 0, 0),
};
_allLinkes.Add(_linkLeftThigh);
_allShapes.Add(_linkLeftThighShape);
// 左脛
_linkLeftShin = new Link() {
LinkId = 2,
ParentLinkId = 1,
Position = new Vec4(0 / scale, 0 / scale, -38.1 / scale),
Orientation = Quaternion.CreateFromAngle(0, 0, 0),
};
var _linkLeftShinShape = new Shape() {
Type = ShapeType.Pyramid,
Scale = new Vec4(-46.5 / scale, 5 / scale, 3 / scale, 0 / scale),
Orientation = Quaternion.CreateFromAngle(0, -Math.PI / 2, 0),
Position = new Vec4(0, 0, 0, 0),
};
_allLinkes.Add(_linkLeftShin);
_allShapes.Add(_linkLeftShinShape);
// 左足首
_linkLeftFoot = new Link() {
LinkId = 3,
ParentLinkId = 2,
Position = new Vec4(0 / scale, 0 / scale, -46.5 / scale),
Orientation = Quaternion.CreateFromAngle(0, 0, 0),
};
var _linkLeftFootShape = new Shape() {
Type = ShapeType.Pyramid,
Scale = new Vec4(16.5 / scale, 2 / scale, 2 / scale, 0 / scale),
Orientation = Quaternion.CreateFromAngle(0, Math.PI / 2, -Math.PI / 2),
Position = new Vec4(0, 0, 0, 0),
};
_allLinkes.Add(_linkLeftFoot);
_allShapes.Add(_linkLeftFootShape);
// 右大腿
_linkRightThigh = new Link() {
LinkId = 4,
ParentLinkId = 0,
Position = new Vec4(-15.4 / scale, 0 / scale, 0 / scale),
Orientation = Quaternion.CreateFromAngle(0, 0, 0),
};
var _linkRightThighShape = new Shape() {
Type = ShapeType.Pyramid,
Scale = new Vec4(-38.1 / scale, 5 / scale, 3 / scale, 0 / scale),
Orientation = Quaternion.CreateFromAngle(0, -Math.PI / 2, 0),
Position = new Vec4(0, 0, 0, 0),
};
_allLinkes.Add(_linkRightThigh);
_allShapes.Add(_linkRightThighShape);
// 右脛
_linkRightShin = new Link() {
LinkId = 5,
ParentLinkId = 4,
Position = new Vec4(0 / scale, 0 / scale, -38.1 / scale),
Orientation = Quaternion.CreateFromAngle(0, 0, 0),
};
var _linkRightShinShape = new Shape() {
Type = ShapeType.Pyramid,
Scale = new Vec4(-46.5 / scale, 5 / scale, 3 / scale, 0 / scale),
Orientation = Quaternion.CreateFromAngle(0, -Math.PI / 2, 0),
Position = new Vec4(0, 0, 0, 0),
};
_allLinkes.Add(_linkRightShin);
_allShapes.Add(_linkRightShinShape);
// 右足首
_linkRightFoot = new Link() {
LinkId = 6,
ParentLinkId = 5,
Position = new Vec4(0 / scale, 0 / scale, -46.5 / scale),
Orientation = Quaternion.CreateFromAngle(0, 0, 0),
};
var _linkRightFootShape = new Shape() {
Type = ShapeType.Pyramid,
Scale = new Vec4(16.5 / scale, 2 / scale, 2 / scale, 0 / scale),
Orientation = Quaternion.CreateFromAngle(0, Math.PI / 2, -Math.PI / 2),
Position = new Vec4(0, 0, 0, 0),
};
_allLinkes.Add(_linkRightFoot);
_allShapes.Add(_linkRightFootShape);
#endregion
// キネマティクスの初期化
_kvm = new KinematicsViewModel(new ForwardKinematicsSolver());
_kvm.Init(_allLinkes, _allShapes);
// 描画領域に追加
foreach (var s in _allShapes) { imuzDraw1.AddPyramidModel((float)s.Scale.x, (float)s.Scale.y, (float)s.Scale.z); }
return;
}
/// <inheritdoc />
public Task <string> ReadMessageAsync(ICommunicationPort serialPort, CancellationToken cancellationToken = default(CancellationToken))
{
return(serialPort.ReadLineAsync());
}
protected override Task <byte> GetQuaternionCountAsync(ICommunicationPort serialPort, CancellationToken cancellationToken)
{
return(Task.FromResult((byte)HarcodedSize));
}
/// <inheritdoc />
public async Task <int> ReadMessageAsync(ICommunicationPort serialPort, CancellationToken cancellationToken = default(CancellationToken))
{
await serialPort.ReadAsync(IntBuffer, 0, sizeof(int), cancellationToken);
return(Unsafe.ReadUnaligned <int>(ref IntBuffer[0]));
}