glax/OSCCollar
1
0
Fork 0
Code Issues Releases 1 Activity

Add Config-Object, cleanup code

Browse Source
This commit is contained in:
Glax 2024-01-02 16:41:14 +01:00
parent d90f95f50b
commit f522950ef7
3 changed files with 112 additions and 85 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
OSCCollar/Config.cs Normal file
View File

@ -0,0 +1,12 @@
namespace VRC_Console;
internal class Config
{
internal string Ip = "127.0.0.1";
internal int PortSend = 9000;
internal double Radius = 100;
internal double CalibrationX = 0;
internal double CalibrationY = 0;
internal double WalkStretchDeadzone = 0.1;
internal double RunStretch = 0.4;
}

182
OSCCollar/OSCCollar.cs
View File

@ -1,6 +1,6 @@
using System.Net; using System.Net;
using BuildSoft.OscCore; using BuildSoft.OscCore;
using Newtonsoft.Json.Linq; using Newtonsoft.Json;
using VRC.OSCQuery; using VRC.OSCQuery;
using Extensions = VRC.OSCQuery.Extensions; using Extensions = VRC.OSCQuery.Extensions;
@ -8,34 +8,35 @@ namespace VRC_Console;
public class OSCCollar public class OSCCollar
{ {
private static readonly string ConfigFilePath = Path.Combine(Directory.GetCurrentDirectory(), "config.json");
public static void Main(string[] args) public static void Main(string[] args)
{ {
string configFilePath = Path.Combine(Directory.GetCurrentDirectory(), "config.json"); OSCCollar _;
if (File.Exists(configFilePath)) if (File.Exists(ConfigFilePath))
{ {
var jObject = JObject.Parse(File.ReadAllText(configFilePath)); Config? config = JsonConvert.DeserializeObject<Config>(File.ReadAllText(ConfigFilePath));
string ip = jObject.GetValue("ip")?.ToObject<string>() ?? "127.0.0.1"; if(config is { })
int portSend = jObject.GetValue("portSend")?.ToObject<int>() ?? 9000; {
double radius = jObject.GetValue("radius")?.ToObject<double>() ?? 100; _ = new OSCCollar(config);
double calibrationX = jObject.GetValue("calibrationX")?.ToObject<double>() ?? 0; }
double calibrationY = jObject.GetValue("calibrationY")?.ToObject<double>() ?? 0; else
double walkStretchDeadzone = jObject.GetValue("walkStretchDeadzone")?.ToObject<double>() ?? 0.1; {
double runStretch = jObject.GetValue("runStretch")?.ToObject<double>() ?? 0.4; _ = new OSCCollar();
bool skipSetup = jObject.GetValue("skipSetup")?.ToObject<bool>() ?? true; }
var _ = new OSCCollar(ip, portSend, radius, calibrationX, calibrationY, walkStretchDeadzone, runStretch, skipSetup);
}else }else
{ {
var _ = new OSCCollar(); _ = new OSCCollar();
} }
} }
private readonly string _ip; private Config _config;
private readonly int _portReceive, _portSend; private readonly int _portReceive;
private OSCQueryService? OscQueryService { get; set; } private OSCQueryService? OscQueryService { get; set; }
private OscServer Server { get; init; } private OscServer _server = null!;
private OscClient Client { get; init; } private OscClient _client = null!;
private float _leashStretch; private float _leashStretch;
private readonly Vector _unitVectorLeash = new(1, 0); private readonly Vector _unitVectorLeash = new(1, 0);
private Vector _movementVector = new(0, 0); private Vector _movementVector = new(0, 0);
@ -46,73 +47,57 @@ public class OSCCollar
private static readonly TimeSpan ConsoleUpdateInterval = TimeSpan.FromMilliseconds(500); private static readonly TimeSpan ConsoleUpdateInterval = TimeSpan.FromMilliseconds(500);
private static readonly TimeSpan UpdateInterval = TimeSpan.FromMilliseconds(10); private static readonly TimeSpan UpdateInterval = TimeSpan.FromMilliseconds(10);
private static readonly TimeSpan MessageMinInterval = TimeSpan.FromMilliseconds(400); private static readonly TimeSpan MessageMinInterval = TimeSpan.FromMilliseconds(400);
private readonly double _radius, _walkStretch, _runStretch; private GPS _gps1 = null!, _gps2 = null!, _gps3 = null!;
private GPS GPS1 { get; init; }
private GPS GPS2 { get; init; }
private GPS GPS3 { get; init; }
private double CalibrationX { get; init; }
private double CalibrationY { get; init; }
private readonly Queue<ValueTuple<double, double>> _calibrationAverage = new(); private readonly Queue<ValueTuple<double, double>> _calibrationAverage = new();
private string _debugValue = ""; private string _debugValue = "";
private double GPSConstantA { get; init; } private double _constantA, _constantB, _constantC, _constantD, _constantE;
private double GPSConstantB { get; init; }
private double GPSConstantC { get; init; }
private double GPSConstantD { get; init; }
private double GPSConstantE { get; init; }
private OSCCollar(string ip = "127.0.0.1", int portSend = 9000, double radius = 100, double calibrationX = 0, double calibrationY = 0, double walkStretch = 0.1, double runStretch = 0.4, bool skipSetup = true) private OSCCollar(Config config, bool skipSetup = true)
{ {
this._ip = ip; this._config = config;
this._portReceive = Extensions.GetAvailableUdpPort(); File.WriteAllText(ConfigFilePath, JsonConvert.SerializeObject(this._config));
this._portSend = portSend;
this._walkStretch = walkStretch;
this._runStretch = runStretch;
this._radius = radius;
this.GPS1 = new(0, -radius);
this.GPS2 = new(-(radius * 0.866), radius / 2);
this.GPS3 = new(radius * 0.866, radius / 2);
this.CalibrationX = calibrationX;
this.CalibrationY = calibrationY;
this.GPSConstantA = 2 * GPS2.X - 2 * GPS1.X;
this.GPSConstantB = 2 * GPS2.Y - 2 * GPS1.Y;
this.GPSConstantC = Math.Pow(GPS1.X, 2) + Math.Pow(GPS2.X, 2) - Math.Pow(GPS1.Y, 2) + Math.Pow(GPS2.Y, 2);
this.GPSConstantD = 2 * GPS3.X - 2 * GPS2.X;
this.GPSConstantE = Math.Pow(GPS2.X, 2) + Math.Pow(GPS3.X, 2) - Math.Pow(GPS2.Y, 2) + Math.Pow(GPS3.Y, 2);
this.SetupGPSVars();
if (!skipSetup) if (!skipSetup)
{ {
Console.WriteLine("Position your GPS receivers:"); Console.WriteLine("Position your GPS receivers:");
Console.WriteLine($"GPS 1 x: {GPS1.X} y: {GPS1.Y}"); Console.WriteLine($"GPS 1 x: {_gps1.X} y: {_gps1.Y}");
Console.WriteLine($"GPS 2 x: {GPS2.X} y: {GPS2.Y}"); Console.WriteLine($"GPS 2 x: {_gps2.X} y: {_gps2.Y}");
Console.WriteLine($"GPS 3 x: {GPS3.X} y: {GPS3.Y}"); Console.WriteLine($"GPS 3 x: {_gps3.X} y: {_gps3.Y}");
Console.WriteLine($"Radius of each receiver (sphere): {radius * 2}"); Console.WriteLine($"Radius of each receiver (sphere): {this._config.Radius * 2}");
Console.ReadKey(); Console.ReadKey();
} }
this.Server = new OscServer(this._portReceive); this._portReceive = Extensions.GetAvailableUdpPort();
this.Client = new OscClient(this._ip, this._portSend); this.SetupOSCServer();
this.Server.TryAddMethod("/avatar/parameters/GPS1", GPS1Handle);
this.Server.TryAddMethod("/avatar/parameters/GPS2", GPS2Handle);
this.Server.TryAddMethod("/avatar/parameters/GPS3", GPS3Handle);
this.Server.TryAddMethod("/avatar/parameters/Leash_Stretch", CollarStretchHandle);
this.Server.TryAddMethod("/avatar/parameters/Leash_Toggle", AllowMovingHandle);
this.Server.Start();
Thread runningThread = new Thread(RunningThread);
runningThread.Start();
this.SetupOSCQuery(); this.SetupOSCQuery();
} }
private OSCCollar(string ip = "127.0.0.1", int portSend = 9000, double radius = 100, double calibrationX = 0,
double calibrationY = 0, double walkStretchDeadzone = 0.1, double runStretch = 0.4,
bool skipSetup = true) : this(new Config()
{
Ip = ip,
PortSend = portSend,
WalkStretchDeadzone = walkStretchDeadzone,
RunStretch = runStretch,
Radius = radius,
CalibrationX = calibrationX,
CalibrationY = calibrationY
}, skipSetup)
{
}
private void PrintOutput() private void PrintOutput()
{ {
Console.Clear(); Console.Clear();
Console.ForegroundColor = ConsoleColor.White; Console.ForegroundColor = ConsoleColor.White;
Console.WriteLine($"OSC Collar - Status: {(_allowMoving ? "enabled" : "disabled")} IP: {_ip} Send-OSC: {_portSend} Receive-OSC: {_portReceive} OSC-HTTP: {this.OscQueryService?.TcpPort}"); Console.WriteLine($"OSC Collar - Status: {(_allowMoving ? "enabled" : "disabled")} IP: {this._config.Ip} Send-OSC: {this._config.PortSend} Receive-OSC: {_portReceive} OSC-HTTP: {this.OscQueryService?.TcpPort}");
Console.WriteLine("=============================="); Console.WriteLine("==============================");
Console.WriteLine($"GPS 1:................{GPS1.Distance:00.00000}"); Console.WriteLine($"GPS 1:................{_gps1.Distance:00.00000}");
Console.WriteLine($"GPS 2:................{GPS2.Distance:00.00000}"); Console.WriteLine($"GPS 2:................{_gps2.Distance:00.00000}");
Console.WriteLine($"GPS 3:................{GPS3.Distance:00.00000}"); Console.WriteLine($"GPS 3:................{_gps3.Distance:00.00000}");
Console.ForegroundColor = ConsoleColor.Cyan; Console.ForegroundColor = ConsoleColor.Cyan;
Console.WriteLine($"Position Vector:......{_unitVectorLeash}"); Console.WriteLine($"Position Vector:......{_unitVectorLeash}");
Console.ForegroundColor = ConsoleColor.White; Console.ForegroundColor = ConsoleColor.White;
@ -165,12 +150,40 @@ public class OSCCollar
Console.WriteLine(_debugValue); Console.WriteLine(_debugValue);
} }
#region Setup
private void SetupGPSVars()
{
this._gps1 = new(0, -this._config.Radius);
this._gps2 = new(-(this._config.Radius * 0.866), this._config.Radius / 2);
this._gps3 = new(this._config.Radius * 0.866, this._config.Radius / 2);
this._constantA = 2 * _gps2.X - 2 * _gps1.X;
this._constantB = 2 * _gps2.Y - 2 * _gps1.Y;
this._constantC = Math.Pow(_gps1.X, 2) + Math.Pow(_gps2.X, 2) - Math.Pow(_gps1.Y, 2) + Math.Pow(_gps2.Y, 2);
this._constantD = 2 * _gps3.X - 2 * _gps2.X;
this._constantE = Math.Pow(_gps2.X, 2) + Math.Pow(_gps3.X, 2) - Math.Pow(_gps2.Y, 2) + Math.Pow(_gps3.Y, 2);
}
private void SetupOSCServer()
{
this._server = new OscServer(this._portReceive);
this._client = new OscClient(this._config.Ip, this._config.PortSend);
this._server.TryAddMethod("/avatar/parameters/GPS1", GPS1Handle);
this._server.TryAddMethod("/avatar/parameters/GPS2", GPS2Handle);
this._server.TryAddMethod("/avatar/parameters/GPS3", GPS3Handle);
this._server.TryAddMethod("/avatar/parameters/Leash_Stretch", CollarStretchHandle);
this._server.TryAddMethod("/avatar/parameters/Leash_Toggle", AllowMovingHandle);
this._server.Start();
Thread runningThread = new Thread(RunningThread);
runningThread.Start();
}
private void SetupOSCQuery() private void SetupOSCQuery()
{ {
var tcpPort = Extensions.GetAvailableTcpPort(); var tcpPort = Extensions.GetAvailableTcpPort();
this.OscQueryService = new OSCQueryServiceBuilder() this.OscQueryService = new OSCQueryServiceBuilder()
.WithHostIP(IPAddress.Parse(this._ip)) .WithHostIP(IPAddress.Parse(this._config.Ip))
.WithOscIP(IPAddress.Parse(this._ip)) .WithOscIP(IPAddress.Parse(this._config.Ip))
.WithDiscovery(new MeaModDiscovery()) .WithDiscovery(new MeaModDiscovery())
.WithTcpPort(tcpPort) .WithTcpPort(tcpPort)
.WithUdpPort(this._portReceive) .WithUdpPort(this._portReceive)
@ -186,6 +199,7 @@ public class OSCCollar
this.OscQueryService.AddEndpoint<bool>("/avatar/parameters/Leash_Toggle", Attributes.AccessValues.WriteOnly); this.OscQueryService.AddEndpoint<bool>("/avatar/parameters/Leash_Toggle", Attributes.AccessValues.WriteOnly);
this.OscQueryService.RefreshServices(); this.OscQueryService.RefreshServices();
} }
#endregion
#region Handle OSC-Messages #region Handle OSC-Messages
private void AllowMovingHandle(OscMessageValues messageValues) private void AllowMovingHandle(OscMessageValues messageValues)
@ -195,17 +209,17 @@ public class OSCCollar
private void GPS1Handle(OscMessageValues messageValues) private void GPS1Handle(OscMessageValues messageValues)
{ {
this.GPS1.Distance = messageValues.ReadFloatElement(0) * _radius; this._gps1.Distance = messageValues.ReadFloatElement(0) * this._config.Radius;
} }
private void GPS2Handle(OscMessageValues messageValues) private void GPS2Handle(OscMessageValues messageValues)
{ {
this.GPS2.Distance = messageValues.ReadFloatElement(0) * _radius; this._gps2.Distance = messageValues.ReadFloatElement(0) * this._config.Radius;
} }
private void GPS3Handle(OscMessageValues messageValues) private void GPS3Handle(OscMessageValues messageValues)
{ {
this.GPS3.Distance = messageValues.ReadFloatElement(0) * _radius; this._gps3.Distance = messageValues.ReadFloatElement(0) * this._config.Radius;
} }
private void CollarStretchHandle(OscMessageValues messageValues) private void CollarStretchHandle(OscMessageValues messageValues)
@ -216,25 +230,25 @@ public class OSCCollar
private void CalculatePositionFromGPS() private void CalculatePositionFromGPS()
{ {
double gpsFactorA = Math.Pow(GPS1.Distance, 2) - Math.Pow(GPS2.Distance, 2) - GPSConstantC; double gpsFactorA = Math.Pow(_gps1.Distance, 2) - Math.Pow(_gps2.Distance, 2) - _constantC;
double gpsFactorB = Math.Pow(GPS2.Distance, 2) - Math.Pow(GPS3.Distance, 2) - GPSConstantE; double gpsFactorB = Math.Pow(_gps2.Distance, 2) - Math.Pow(_gps3.Distance, 2) - _constantE;
double gpsPosX = (gpsFactorB / GPSConstantD); double gpsPosX = (gpsFactorB / _constantD);
double gpsPosY = (gpsFactorA * GPSConstantD - GPSConstantA * gpsFactorB) / (GPSConstantB * GPSConstantD); double gpsPosY = (gpsFactorA * _constantD - _constantA * gpsFactorB) / (_constantB * _constantD);
AddCalibrationValues(gpsPosX, gpsPosY); AddCalibrationValues(gpsPosX, gpsPosY);
double posX = gpsPosX + CalibrationX; double posX = gpsPosX + this._config.CalibrationX;
double posY = gpsPosY + CalibrationY; double posY = gpsPosY + this._config.CalibrationY;
double inverseLength = 1 / Math.Sqrt(Math.Pow(posX, 2) + Math.Pow(posY, 2)); double inverseLength = 1 / Math.Sqrt(Math.Pow(posX, 2) + Math.Pow(posY, 2));
_unitVectorLeash.X = posX * inverseLength * -1; _unitVectorLeash.X = posX * inverseLength * -1;
_unitVectorLeash.Y = posY * inverseLength; _unitVectorLeash.Y = posY * inverseLength;
if (_leashStretch < _walkStretch) //Below Deadzone if (_leashStretch < this._config.WalkStretchDeadzone) //Below Deadzone
_movementVector = new(); _movementVector = new();
else if (_leashStretch < _runStretch) //Walk else if (_leashStretch < this._config.RunStretch) //Walk
_movementVector = _unitVectorLeash.MultipliedWith(0.5); _movementVector = _unitVectorLeash.MultipliedWith(0.5);
else //Run else //Run
_movementVector = _unitVectorLeash.MultipliedWith(1); _movementVector = _unitVectorLeash.MultipliedWith(1);
@ -244,18 +258,18 @@ public class OSCCollar
{ {
while (true) while (true)
{ {
this.Server.Update(); this._server.Update();
CalculatePositionFromGPS(); CalculatePositionFromGPS();
if (_lastNilMessageSent.Add(MessageMinInterval) < DateTime.Now) if (_lastNilMessageSent.Add(MessageMinInterval) < DateTime.Now)
{ {
this.Client.Send("/input/Vertical", 0f); this._client.Send("/input/Vertical", 0f);
this.Client.Send("/input/Horizontal", 0f); this._client.Send("/input/Horizontal", 0f);
this._lastNilMessageSent = DateTime.Now; this._lastNilMessageSent = DateTime.Now;
_nilSentCount++; _nilSentCount++;
}else if (_allowMoving) }else if (_allowMoving)
{ {
this.Client.Send("/input/Vertical", Convert.ToSingle(_movementVector.Y)); this._client.Send("/input/Vertical", Convert.ToSingle(_movementVector.Y));
this.Client.Send("/input/Horizontal", Convert.ToSingle(_movementVector.X)); this._client.Send("/input/Horizontal", Convert.ToSingle(_movementVector.X));
} }
if (_lastConsoleOutput.Add(ConsoleUpdateInterval) < DateTime.Now) if (_lastConsoleOutput.Add(ConsoleUpdateInterval) < DateTime.Now)

1
OSCCollar/OSCCollar.csproj
View File

@ -13,6 +13,7 @@
<PackageReference Include="MeaMod.DNS" Version="1.0.70" /> <PackageReference Include="MeaMod.DNS" Version="1.0.70" />
<PackageReference Include="Microsoft.Extensions.Logging" Version="8.0.0" /> <PackageReference Include="Microsoft.Extensions.Logging" Version="8.0.0" />
<PackageReference Include="Newtonsoft.Json" Version="13.0.3" /> <PackageReference Include="Newtonsoft.Json" Version="13.0.3" />
<PackageReference Include="Spectre.Console" Version="0.48.1-preview.0.7" />
</ItemGroup> </ItemGroup>
<ItemGroup> <ItemGroup>