464 lines
16 KiB
C#
464 lines
16 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Collections.Concurrent;
|
|
using LinearAlgebra;
|
|
|
|
namespace RoboidControl {
|
|
|
|
/// <summary>
|
|
/// A thing is the primitive building block
|
|
/// </summary>
|
|
[Serializable]
|
|
public class Thing {
|
|
|
|
/// <summary>
|
|
/// Predefined thing types
|
|
/// </summary>
|
|
public static class Type {
|
|
public const byte Undetermined = 0x00;
|
|
// sensor
|
|
public const byte Switch = 0x01;
|
|
public const byte DistanceSensor = 0x02;
|
|
public const byte DirectionalSensor = 0x03;
|
|
public const byte TemperatureSensor = 0x04;
|
|
public const byte TouchSensor = 0x05;
|
|
// Motor
|
|
public const byte ControlledMotor = 0x06;
|
|
public const byte UncontrolledMotor = 0x07;
|
|
public const byte Servo = 0x08;
|
|
public const byte RelativeEncoder = 0x19;
|
|
// Other
|
|
public const byte Root = 0x10;
|
|
public const byte Roboid = 0x09;
|
|
public const byte Humanoid = 0x0A;
|
|
public const byte ExternalSensor = 0x0B;
|
|
public const byte Animator = 0x0C;
|
|
public const byte DifferentialDrive = 0x0D;
|
|
}
|
|
|
|
#region Init
|
|
|
|
private Thing(Participant owner) {
|
|
this.type = Type.Root;
|
|
|
|
this.positionUpdated = true;
|
|
this.orientationUpdated = true;
|
|
this.hierarchyChanged = true;
|
|
|
|
this.owner = owner;
|
|
this.parent = null;
|
|
}
|
|
|
|
public static Thing CreateRoot(Participant owner) {
|
|
return new Thing(owner);
|
|
}
|
|
|
|
static Thing localRoot {
|
|
get {
|
|
Participant participant = Participant.localParticipant;
|
|
return participant.root;
|
|
}
|
|
}
|
|
|
|
/*
|
|
/// <summary>
|
|
/// Create a new thing without communication abilities
|
|
/// </summary>
|
|
/// <param name="thingType">The type of thing (can use \ref RoboidControl::Thing::Type "Thing.Type")</param>
|
|
/// <param name="invokeEvent">Invoke a OnNewThing event when the thing has been created</param>
|
|
public Thing(byte thingType = Type.Undetermined, bool invokeEvent = true) : this(ParticipantUDP.Isolated(), thingType, 0, invokeEvent) {
|
|
}
|
|
|
|
/// <summary>
|
|
/// Create a new thing for a participant
|
|
/// </summary>
|
|
/// <param name="owner">The owning participant</param>
|
|
/// <param name="thingType">The type of thing (can use \ref RoboidControl::Thing::Type "Thing.Type")</param>
|
|
/// <param name="thingId">The ID of the thing, leave out or set to zero to generate an ID</param>
|
|
/// <param name="invokeEvent">Invoke a OnNewThing event when the thing has been created</param>
|
|
public Thing(Participant owner, byte thingType = Type.Undetermined, byte thingId = 0, bool invokeEvent = true) {
|
|
this.owner = owner;
|
|
this.id = thingId;
|
|
this.type = thingType;
|
|
if (this.owner != null)
|
|
this.owner.Add(this);
|
|
if (invokeEvent) {
|
|
Participant.UpdateEvent e = new() {
|
|
messageId = ThingMsg.id,
|
|
thing = this
|
|
};
|
|
this.owner.updateQueue.Enqueue(e);
|
|
}
|
|
}
|
|
*/
|
|
|
|
/// <summary>
|
|
/// Create a new child thing
|
|
/// </summary>
|
|
/// <param name="parent">The parent thing</param>
|
|
/// <param name="thingType">The type of thing (can use \ref RoboidControl::Thing::Type "Thing.Type")</param>
|
|
/// <param name="thingId">The ID of the thing, leave out or set to zero to generate an ID</param>
|
|
/// <param name="invokeEvent">Invoke a OnNewThing event when the thing has been created</param>
|
|
/// <note>The owner will be the same as the owner of the parent thing</note>
|
|
/*
|
|
public Thing(Thing parent, byte thingType = Type.Undetermined, byte thingId = 0, bool invokeEvent = true) : this(parent.owner, thingType, thingId, invokeEvent) {
|
|
this.parent = parent;
|
|
}
|
|
*/
|
|
public Thing(Thing parent = default) {
|
|
this.type = Type.Undetermined;
|
|
|
|
this.positionUpdated = true;
|
|
this.orientationUpdated = true;
|
|
this.hierarchyChanged = true;
|
|
|
|
if (parent == default)
|
|
this.parent = Participant.localParticipant.root;
|
|
else
|
|
this.parent = parent;
|
|
|
|
this.owner = this.parent.owner;
|
|
this.owner.Add(this, true);
|
|
|
|
Participant.UpdateEvent e = new() {
|
|
messageId = ThingMsg.id,
|
|
thing = this
|
|
};
|
|
this.owner.updateQueue.Enqueue(e);
|
|
}
|
|
|
|
// public static Thing CreateRemote(Participant owner, byte thingId) {
|
|
// Thing remoteThing = new(owner.root) {
|
|
// id = thingId
|
|
// };
|
|
// return remoteThing;
|
|
// }
|
|
|
|
/// <summary>
|
|
/// Function which can be used to create components in external engines.
|
|
/// </summary>
|
|
/// Currently this is used to create GameObjects in Unity
|
|
public virtual void CreateComponent() {
|
|
#if UNITY_5_3_OR_NEWER
|
|
this.component = Unity.Thing.Create(this);
|
|
this.component.core = this;
|
|
#endif
|
|
}
|
|
|
|
#endregion Init
|
|
|
|
public bool terminate = false;
|
|
|
|
#region Properties
|
|
|
|
/// <summary>
|
|
/// The participant owning this thing
|
|
/// </summary>
|
|
public Participant owner = null;
|
|
/// <summary>
|
|
/// The ID of this thing
|
|
/// </summary>
|
|
public byte id = 0;
|
|
|
|
/// <summary>
|
|
/// The type of this thing.
|
|
/// </summary>
|
|
/// This can be either a \ref RoboidControl::Thing::Type "Thing.Type" or a byte value for custom types.
|
|
public byte type = Type.Undetermined;
|
|
|
|
private string _name = "";
|
|
/// <summary>
|
|
/// The name of the thing
|
|
/// </summary>
|
|
public virtual string name {
|
|
get => _name;
|
|
set {
|
|
if (_name != value) {
|
|
_name = value;
|
|
nameChanged = true;
|
|
this.updateQueue.Enqueue(new CoreEvent(NameMsg.Id));
|
|
}
|
|
}
|
|
}
|
|
public bool nameChanged = false;
|
|
|
|
private string _modelUrl = "";
|
|
/// <summary>
|
|
/// An URL pointing to the location where a model of the thing can be found
|
|
/// </summary>
|
|
public string modelUrl {
|
|
get => _modelUrl;
|
|
set {
|
|
if (_modelUrl != value) {
|
|
_modelUrl = value;
|
|
this.updateQueue.Enqueue(new CoreEvent(ModelUrlMsg.Id));
|
|
}
|
|
}
|
|
}
|
|
|
|
#if UNITY_5_3_OR_NEWER
|
|
/// <summary>
|
|
/// A reference to the representation of the thing in Unity
|
|
/// </summary>
|
|
[NonSerialized]
|
|
public Unity.Thing component = null;
|
|
#endif
|
|
|
|
#endregion Properties
|
|
|
|
#region Hierarchy
|
|
|
|
private Thing _parent;
|
|
/// <summary>
|
|
/// The parent of this thing
|
|
/// </summary>
|
|
public Thing parent {
|
|
get => _parent;
|
|
set {
|
|
if (_parent == value)
|
|
return;
|
|
|
|
if (value == null) {
|
|
_parent?.RemoveChild(this);
|
|
_parent = null;
|
|
}
|
|
else {
|
|
value.AddChild(this);
|
|
}
|
|
this.hierarchyChanged = true;
|
|
this.updateQueue.Enqueue(new CoreEvent(ThingMsg.id));
|
|
}
|
|
}
|
|
|
|
|
|
/// <summary>
|
|
/// The children of this thing
|
|
/// </summary>
|
|
[NonSerialized]
|
|
protected List<Thing> children = new();
|
|
|
|
/// <summary>
|
|
/// Add a child Thing to this Thing
|
|
/// </summary>
|
|
/// <param name="child">The Thing which should become a child</param>
|
|
/// @remark When the Thing is already a child, it will not be added again
|
|
public void AddChild(Thing child) {
|
|
if (children.Find(thing => thing == child) != null)
|
|
return;
|
|
|
|
child._parent = this;
|
|
children.Add(child);
|
|
}
|
|
/// <summary>
|
|
/// Remove the given thing as a child of this thing
|
|
/// </summary>
|
|
/// <param name="child">The child to remove</param>
|
|
/// <returns>True when the child was present or false when it was not found</returns>
|
|
public bool RemoveChild(Thing child) {
|
|
return children.Remove(child);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get a child by thing Id
|
|
/// </summary>
|
|
/// <param name="thingId">The thing ID to find</param>
|
|
/// <param name="recurse">Look recursively through all descendants</param>
|
|
/// <returns></returns>
|
|
Thing GetChild(byte thingId, bool recurse = false) {
|
|
foreach (Thing child in this.children) {
|
|
if (child == null)
|
|
continue;
|
|
|
|
if (child.id == thingId)
|
|
return child;
|
|
if (recurse) {
|
|
Thing foundChild = child.GetChild(thingId, recurse);
|
|
if (foundChild != null)
|
|
return foundChild;
|
|
}
|
|
}
|
|
return null;
|
|
}
|
|
/// <summary>
|
|
/// Find a child by name
|
|
/// </summary>
|
|
/// <param name="name">The name of the child thing</param>
|
|
/// <param name="recurse">Look recursively through all descendants</param>
|
|
/// <returns>The found thing or null when nothing is found</returns>
|
|
Thing FindChild(string name, bool recurse = true) {
|
|
foreach (Thing child in this.children) {
|
|
if (child == null)
|
|
continue;
|
|
|
|
if (child.name == name)
|
|
return child;
|
|
|
|
if (recurse) {
|
|
Thing foundChild = child.FindChild(name, recurse);
|
|
if (foundChild != null)
|
|
return foundChild;
|
|
}
|
|
}
|
|
return null;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Indicator that the hierarchy of the thing has changed
|
|
/// </summary>
|
|
public bool hierarchyChanged = true;
|
|
|
|
#endregion Hierarchy
|
|
|
|
#region Pose
|
|
|
|
private Spherical _position = Spherical.zero;
|
|
/// <summary>
|
|
/// The position of the thing in local space, in meters.
|
|
/// </summary>
|
|
public Spherical position {
|
|
get { return _position; }
|
|
set {
|
|
if (_position != value) {
|
|
_position = value;
|
|
positionUpdated = true;
|
|
updateQueue.Enqueue(new CoreEvent(PoseMsg.Id));
|
|
}
|
|
}
|
|
}
|
|
/// <summary>
|
|
/// Boolean indicating that the thing has an updated position
|
|
/// </summary>
|
|
public bool positionUpdated = false;
|
|
|
|
private SwingTwist _orientation = SwingTwist.zero;
|
|
/// <summary>
|
|
/// The orientation of the thing in local space
|
|
/// </summary>
|
|
public SwingTwist orientation {
|
|
get { return _orientation; }
|
|
set {
|
|
if (_orientation != value) {
|
|
_orientation = value;
|
|
orientationUpdated = true;
|
|
updateQueue.Enqueue(new CoreEvent(PoseMsg.Id));
|
|
//OnOrientationChanged?.Invoke();
|
|
}
|
|
}
|
|
}
|
|
/// <summary>
|
|
/// Boolean indicating the thing has an updated orientation
|
|
/// </summary>
|
|
public bool orientationUpdated = false;
|
|
|
|
private Spherical _linearVelocity = Spherical.zero;
|
|
/// <summary>
|
|
/// The linear velocity of the thing in local space in meters per second
|
|
/// </summary>
|
|
public Spherical linearVelocity {
|
|
get => _linearVelocity;
|
|
set {
|
|
if (_linearVelocity != value) {
|
|
_linearVelocity = value;
|
|
linearVelocityUpdated = true;
|
|
updateQueue.Enqueue(new CoreEvent(PoseMsg.Id));
|
|
}
|
|
}
|
|
}
|
|
/// <summary>
|
|
/// Boolean indicating the thing has an updated linear velocity
|
|
/// </summary>
|
|
public bool linearVelocityUpdated = false;
|
|
|
|
private Spherical _angularVelocity = Spherical.zero;
|
|
/// <summary>
|
|
/// The angular velocity of the thing in local space in degrees per second
|
|
/// </summary>
|
|
public Spherical angularVelocity {
|
|
get => _angularVelocity;
|
|
set {
|
|
if (_angularVelocity != value) {
|
|
_angularVelocity = value;
|
|
angularVelocityUpdated = true;
|
|
updateQueue.Enqueue(new CoreEvent(PoseMsg.Id));
|
|
}
|
|
}
|
|
}
|
|
/// <summary>
|
|
/// Boolean indicating the thing has an updated angular velocity
|
|
/// </summary>
|
|
public bool angularVelocityUpdated = false;
|
|
|
|
#endregion Pose
|
|
|
|
#region Update
|
|
|
|
/// <summary>
|
|
/// Get the current time in milliseconds
|
|
/// </summary>
|
|
/// <returns>The current time in milliseconds</returns>
|
|
public static ulong GetTimeMs() {
|
|
#if UNITY_5_3_OR_NEWER
|
|
return (ulong)(UnityEngine.Time.time * 1000);
|
|
#else
|
|
return TimeManager.GetCurrentTimeMilliseconds();
|
|
#endif
|
|
}
|
|
|
|
/// <summary>
|
|
/// Update de state of the thing
|
|
/// </summary>
|
|
/// <param name="recurse">When true, this will Update the descendants recursively</param>
|
|
public void Update(bool recurse = false) {
|
|
Update(GetTimeMs(), recurse);
|
|
}
|
|
// #endif
|
|
/// <summary>
|
|
/// Update this thing
|
|
/// </summary>
|
|
/// <param name="currentTime">he current clock time in milliseconds; if this is zero, the current time is retrieved automatically</param>
|
|
/// <param name="recurse">When true, this will Update the descendants recursively</param>
|
|
public virtual void Update(ulong currentTimeMs, bool recurse = false) {
|
|
this.positionUpdated = false;
|
|
this.orientationUpdated = false;
|
|
this.linearVelocityUpdated = false;
|
|
this.angularVelocityUpdated = false;
|
|
this.hierarchyChanged = false;
|
|
|
|
// should recurse over children...
|
|
if (recurse) {
|
|
for (byte childIx = 0; childIx < this.children.Count; childIx++) {
|
|
Thing child = this.children[childIx];
|
|
if (child == null)
|
|
continue;
|
|
child.Update(currentTimeMs, recurse);
|
|
}
|
|
}
|
|
}
|
|
|
|
public class CoreEvent {
|
|
public CoreEvent(int messageId) {
|
|
this.messageId = messageId;
|
|
}
|
|
public int messageId; // see the communication messages
|
|
};
|
|
public ConcurrentQueue<CoreEvent> updateQueue = new();
|
|
|
|
#endregion Update
|
|
|
|
/// <summary>
|
|
/// Function used to generate binary data for this thing
|
|
/// </summary>
|
|
/// <returns>A byte array with the binary data</returns>
|
|
/// @sa Passer::RoboidControl::BinaryMsg
|
|
public virtual byte[] GenerateBinary() { return Array.Empty<byte>(); }
|
|
|
|
/// <summary>
|
|
/// Function used to process binary data received for this thing
|
|
/// </summary>
|
|
/// <param name="bytes">The binary data to process</param>
|
|
public virtual void ProcessBinary(byte[] bytes) {
|
|
}
|
|
|
|
}
|
|
}
|