RoboidControl/RoboidControl-csharp
RoboidControl/RoboidControl-csharp
1
0
Fork 0
Code Issues 1 Pull Requests Actions Packages Releases Activity

Compare commits

base: RoboidControl:620b7e52734a790bb05075546fc29a460430ee97
Branches Tags
RoboidControl:main
RoboidControl:v0.4_dev
RoboidControl:v0.3.0
..
compare: RoboidControl:2b2f751658f6cfba958b661367b75bbe11d429f3
Branches Tags
RoboidControl:v0.4_dev
RoboidControl:main
RoboidControl:v0.3.0

No commits in common. "620b7e52734a790bb05075546fc29a460430ee97" and "2b2f751658f6cfba958b661367b75bbe11d429f3" have entirely different histories.
620b7e5273 ... 2b2f751658

4 changed files with 52 additions and 30 deletions
Show Stats Expand all files Collapse all files

21
Unity/Thing.cs
View File

@ -132,10 +132,8 @@ namespace RoboidControl.Unity {
string extension = core.modelUrl[core.modelUrl.LastIndexOf(".")..];
if (extension == ".jpg" || extension == ".png")
StartCoroutine(LoadJPG());
#if GLTF
else if (extension == ".gltf" || extension == ".glb")
ProcessGltfModel(core);
#endif
break;
case PoseMsg.Id:
Debug.Log($"{this.core.id} Handle Pose");
@ -179,9 +177,9 @@ namespace RoboidControl.Unity {
}
}
#if GLTF
bool loadingModel = false;
private async void ProcessGltfModel(RoboidControl.Thing coreThing) {
#if GLTF
if (!loadingModel) {
loadingModel = true;
@ -202,6 +200,21 @@ namespace RoboidControl.Unity {
await gltfImport.InstantiateMainSceneAsync(parentTransform);
SkinnedMeshRenderer[] meshRenderers = parentTransform.GetComponentsInChildren<SkinnedMeshRenderer>();
#if pHUMANOID4
// if (parentThing.objectType == 7) {
// HumanoidControl hc = parentThing.gameObject.GetComponent<HumanoidControl>();
// if (hc == null)
// hc = parentThing.gameObject.AddComponent<HumanoidControl>();
// foreach (SkinnedMeshRenderer meshRenderer in meshRenderers) {
// if (meshRenderer.rootBone != null) {
// Debug.Log("Found a skinned mesh with bones");
// hc.RetrieveBonesFrom(meshRenderer.rootBone);
// break;
// }
// }
// }
#endif
parentTransform.localScale = Vector3.one;
if (meshRenderers.Length > 0) {
foreach (SkinnedMeshRenderer meshRenderer in meshRenderers) {
@ -221,8 +234,8 @@ namespace RoboidControl.Unity {
}
}
loadingModel = true;
}
#endif
}
private void ScanForThings(Transform rootTransform) {
RoboidControl.Thing[] thingArray = this.core.owner.things.ToArray();

5
src/Messages/LowLevelMessages.cs
View File

@ -45,15 +45,18 @@ namespace RoboidControl {
public static void SendQuat32(byte[] buffer, ref byte ix, SwingTwist s) {
Quat32 q32 = Quat32.FromSwingTwist(s);
System.Console.Write($"send q32: {q32.x} {q32.y} {q32.z} {q32.w}");
SendQuat32(buffer, ref ix, q32);
}
public static void SendSwingTwist(byte[] buffer, ref byte ix, SwingTwist r) {
System.Console.Write($"send st: {r.swing.horizontal} {r.swing.vertical} {r.twist}");
SendAngle8(buffer, ref ix, r.swing.horizontal);
SendAngle8(buffer, ref ix, r.swing.vertical);
SendAngle8(buffer, ref ix, r.twist);
}
public static SwingTwist ReceiveSwingTwist(byte[] data, ref byte ix) {
float horizontal = ReceiveAngle8(data, ref ix);
float vertical = ReceiveAngle8(data, ref ix);

54
src/Participants/ParticipantUDP.cs
View File

@ -160,6 +160,7 @@ namespace RoboidControl {
if (this.remoteSite == null)
this.Publish(msg);
else
//this.Send(this.remoteSite, msg);
this.remoteSite.Send(msg);
this.nextPublishMe = currentTimeMS + this.publishIntervalMS;
@ -241,6 +242,12 @@ namespace RoboidControl {
#region Send
public void SendThingInfo(Participant owner, Thing thing) {
// Console.WriteLine("Send thing info");
// this.Send(owner, new ThingMsg(this.networkId, thing));
// this.Send(owner, new NameMsg(this.networkId, thing));
// this.Send(owner, new ModelUrlMsg(this.networkId, thing));
// this.Send(owner, new PoseMsg(this.networkId, thing));
// this.Send(owner, new BinaryMsg(this.networkId, thing));
owner.Send(new ThingMsg(this.networkId, thing));
owner.Send(new NameMsg(this.networkId, thing));
owner.Send(new ModelUrlMsg(this.networkId, thing));
@ -257,12 +264,23 @@ namespace RoboidControl {
this.Publish(new BinaryMsg(this.networkId, thing));
}
// public bool Send(Participant owner, IMessage msg) {
// int bufferSize = msg.Serialize(ref this.buffer);
// if (bufferSize <= 0)
// return true;
// IPEndPoint participantEndpoint = new IPEndPoint(IPAddress.Parse(owner.ipAddress), owner.port);
// // Console.WriteLine($"msg to {participantEndpoint.Address.ToString()} {participantEndpoint.Port}");
// this.udpClient?.Send(this.buffer, bufferSize, participantEndpoint);
// return true;
// }
public bool Publish(IMessage msg) {
int bufferSize = msg.Serialize(ref this.buffer);
if (bufferSize <= 0)
return true;
Console.WriteLine($"publish to {broadcastIpAddress.ToString()} {this.port}");
// Console.WriteLine($"publish to {broadcastIpAddress.ToString()} {this.port}");
this.udpClient?.Send(this.buffer, bufferSize, this.broadcastIpAddress, this.port);
return true;
}
@ -273,35 +291,23 @@ namespace RoboidControl {
protected void ReceiveUDP(IAsyncResult result) {
try {
if (this.udpClient == null)
// UnityEngine.Debug.Log("received");
if (this.udpClient == null) // || this.endPoint == null)
return;
byte[] data = { };
try {
data = this.udpClient.EndReceive(result, ref endPoint);
}
catch (SocketException e) {
if (e.ErrorCode != 10054)
throw;
#if DEBUG
else
Console.WriteLine($"No receiver found for the previously sent UDP packet");
#endif
}
byte[] data = this.udpClient.EndReceive(result, ref endPoint);
// This does not yet take multi-packet messages into account!
if (endPoint == null)
return;
if (data.Length > 0) {
// We can receive our own publish (broadcast) packages. How do we recognize them????
// It is hard to determine our source port
string ipAddress = endPoint.Address.ToString();
if (ipAddress != this.ipAddress) {
Participant remoteParticipant = GetParticipant(ipAddress, endPoint.Port);
remoteParticipant ??= AddParticipant(ipAddress, endPoint.Port, this);
// We can receive our own publish (broadcast) packages. How do we recognize them????
// It is hard to determine our source port
string ipAddress = endPoint.Address.ToString();
if (ipAddress != this.ipAddress) {
Participant remoteParticipant = GetParticipant(ipAddress, endPoint.Port);
remoteParticipant ??= AddParticipant(ipAddress, endPoint.Port, this);
ReceiveData(data, remoteParticipant);
}
ReceiveData(data, remoteParticipant);
}
udpClient.BeginReceive(new AsyncCallback(callbackResult => ReceiveUDP(callbackResult)), null);
@ -431,7 +437,7 @@ namespace RoboidControl {
protected virtual void Process(Participant sender, BinaryMsg msg) {
#if DEBUG
// Console.WriteLine($"{this.name}: Process BinaryMsg [{msg.networkId}/{msg.thingId}] {msg.dataLength}");
// Console.WriteLine($"{this.name}: Process BinaryMsg [{msg.networkId}/{msg.thingId}] {msg.dataLength}");
#endif
Thing thing = sender.Get(msg.thingId);
thing?.ProcessBinary(msg.data);

2
src/Things/EncoderMotor.cs
View File

@ -50,7 +50,7 @@ namespace RoboidControl {
float pidP = 0.1F;
float pidD = 0.0F;
//float pidI = 0.0F;
float pidI = 0.0F;
public override void Update(ulong currentTimeMs, bool recurse = false) {
float actualSpeed = this.encoder.angularSpeed;