diff --git a/Examples/BB2B/Program.cs b/Examples/BB2B/Program.cs index d30d335..d4a57f2 100644 --- a/Examples/BB2B/Program.cs +++ b/Examples/BB2B/Program.cs @@ -1,33 +1,29 @@ -using System; -using System.Diagnostics; -using System.Threading; +using System.Threading; using RoboidControl; class BB2B { static void Main() { // The robot's propulsion is a differential drive - DifferentialDrive bb2b = new DifferentialDrive(); - // Is has a touch sensor at the front left of the roboid - TouchSensor touchLeft = new TouchSensor(bb2b); + DifferentialDrive bb2b = new(); + // It has a touch sensor at the front left of the roboid + TouchSensor touchLeft = new(bb2b); // and other one on the right - TouchSensor touchRight = new TouchSensor(bb2b); + TouchSensor touchRight = new(bb2b); // Do forever: while (true) { - Console.Write("A"); // The left wheel turns forward when nothing is touched on the right side // and turn backward when the roboid hits something on the right - float leftWheelSpeed = (touchRight.touchedSomething) ? -600.0f : 600.0f; + float leftWheelSpeed = touchRight.touchedSomething ? -600.0f : 600.0f; // The right wheel does the same, but instead is controlled by // touches on the left side - float rightWheelSpeed = (touchLeft.touchedSomething) ? -600.0f : 600.0f; + float rightWheelSpeed = touchLeft.touchedSomething ? -600.0f : 600.0f; // When both sides are touching something, both wheels will turn backward // and the roboid will move backwards bb2b.SetWheelVelocity(leftWheelSpeed, rightWheelSpeed); // Update the roboid state bb2b.Update(true); - // and sleep for 100ms Thread.Sleep(100); } diff --git a/RoboidControl-csharp.code-workspace b/RoboidControl-csharp.code-workspace new file mode 100644 index 0000000..876a149 --- /dev/null +++ b/RoboidControl-csharp.code-workspace @@ -0,0 +1,8 @@ +{ + "folders": [ + { + "path": "." + } + ], + "settings": {} +} \ No newline at end of file diff --git a/src/LocalParticipant.cs b/src/LocalParticipant.cs index 3350485..a057bdc 100644 --- a/src/LocalParticipant.cs +++ b/src/LocalParticipant.cs @@ -310,13 +310,16 @@ namespace RoboidControl { //Console.WriteLine($"Participant: Process pose [{msg.networkId}/{msg.thingId}] {msg.poseType}"); Thing thing = sender.Get(msg.networkId, msg.thingId); if (thing != null) { - thing.hasPosition = false; + thing.positionUpdated = false; if ((msg.poseType & PoseMsg.Pose_Position) != 0) { thing.position = msg.position; - thing.hasPosition = true; + thing.positionUpdated = true; } - if ((msg.poseType & PoseMsg.Pose_Orientation) != 0) + thing.orientationUpdated = false; + if ((msg.poseType & PoseMsg.Pose_Orientation) != 0) { thing.orientation = msg.orientation; + thing.orientationUpdated = true; + } else thing.orientation = null; if ((msg.poseType & PoseMsg.Pose_LinearVelocity) != 0) diff --git a/src/Participant.cs b/src/Participant.cs index 53f637e..6c94a19 100644 --- a/src/Participant.cs +++ b/src/Participant.cs @@ -4,29 +4,19 @@ using System.Collections.Generic; namespace RoboidControl { /// - /// A reference to a participant, possibly on a remote location + /// A participant is a device which manages things. /// + /// It can communicate with other participant to synchronise the state of things. + /// This class is used to register the things the participant is managing. + /// It also maintains the communcation information to contact the participant. + /// It is used as a basis for the local participant, but also as a reference to remote participants. public class Participant { - /// - /// The internet address of the participant - /// - public string ipAddress = "0.0.0.0"; - /// - /// The UDP port on which the participant can be reached - /// - public int port = 0; - - /// - /// The network ID of the participant - /// - public byte networkId; - /// /// Default constructor /// public Participant() { } /// - /// Create a new remote participant + /// Create a new participant with the given communcation info /// /// The IP address of the participant /// The UDP port of the participant @@ -36,7 +26,21 @@ namespace RoboidControl { } /// - /// The things reported by this participant + /// The Ip Address of a participant. When the participant is local, this contains 0.0.0.0 + /// + public string ipAddress = "0.0.0.0"; + /// + /// The port number for UDP communication with the participant. This is 0 for isolated participants. + /// + public int port = 0; + + /// + /// The network ID of the participant + /// + public byte networkId; + + /// + /// The things managed by this participant /// protected readonly List things = new List(); @@ -58,7 +62,11 @@ namespace RoboidControl { /// /// The thing to add /// Invoke an notification event when the thing has been added - public void Add(Thing thing, bool invokeEvent = true) { + public void Add(Thing thing, bool checkId = true, bool invokeEvent = true) { + if (checkId && thing.id == 0) { + thing.id = (byte)(this.things.Count + 1); + things.Add(thing); + } // Console.WriteLine($"added thing [{thing.networkId}/{thing.id}]"); Thing foundThing = Get(thing.networkId, thing.id); @@ -69,14 +77,22 @@ namespace RoboidControl { Thing.InvokeNewThing(thing); // Console.Write($"Add thing {ipAddress}:{port}[{networkId}/{thing.id}]"); } - else { - if (thing != foundThing) { - // should be: find first non-existing id... - thing.id = (byte)this.things.Count; - things.Add(thing); - // Console.Write($"Add thing, updated thing id to [{thing.networkId}/{thing.id}]"); - } - } + // else { + // if (thing != foundThing) { + // // should be: find first non-existing id... + // thing.id = (byte)this.things.Count; + // things.Add(thing); + // // Console.Write($"Add thing, updated thing id to [{thing.networkId}/{thing.id}]"); + // } + // } + } + + /// + /// Remove a thing for this participant + /// + /// The thing to remove + public void Remove(Thing thing) { + this.things.Remove(thing); } } diff --git a/src/SiteServer.cs b/src/SiteServer.cs index ce2d178..4d70718 100644 --- a/src/SiteServer.cs +++ b/src/SiteServer.cs @@ -73,7 +73,7 @@ namespace RoboidControl { newThing.parent = parentThing; } Console.WriteLine("Adding to remote sender"); - sender.Add(newThing); + sender.Add(newThing, false, true); } } diff --git a/src/Thing.cs b/src/Thing.cs index bf5b109..42f1f92 100644 --- a/src/Thing.cs +++ b/src/Thing.cs @@ -33,37 +33,57 @@ namespace RoboidControl { ExternalSensor }; + public delegate void ChangeHandler(); + public delegate void SphericalHandler(Spherical v); + public delegate void ThingHandler(Thing t); + #endregion Types #region Init + /// + /// Create a new thing without communication abilities + /// + /// The type of thing public Thing(byte thingType = (byte)Type.Undetermined) : this(LocalParticipant.Isolated(), thingType) { } + /// + /// Create a new thing for a participant + /// + /// The participant owning the thing + /// The type of thing public Thing(Participant owner, byte thingType = (byte)Type.Undetermined) { this.owner = owner; this.type = thingType; } + /// + /// Create a new thing as a child of another thing + /// + /// The parent thing + /// The type of thing public Thing(Thing parent, byte thingType = (byte)Type.Undetermined) : this(parent.owner, thingType) { this.parent = parent; } + /* + /// + /// Create a new thing for the given participant + /// + /// The participant for which this thing is created + /// True when a new thing event should be triggered + public Thing(Participant owner, bool invokeEvent = false) { + this.owner = owner; + if (invokeEvent) + InvokeNewThing(this); + } + */ + /// /// Create a new thing for the given participant /// - /// The participant for which this thing is created - /// True when a new thing event should be triggered - public Thing(Participant owner, bool invokeEvent = false) { - this.owner = owner; - //owner.Add(this); - if (invokeEvent) - InvokeNewThing(this); - } - /// - /// Create a new thing for the given participant - /// - /// The participant for which this thing is created + /// The participant owning the thing /// The network ID of the thing /// The ID of the thing /// The type of thing @@ -89,34 +109,26 @@ namespace RoboidControl { #region Properties - public delegate void ChangeHandler(); - public delegate void SphericalHandler(Spherical v); - public delegate void ThingHandler(Thing t); - /// - /// The participant to which this thing belongs + /// The participant owning this thing /// - public Participant owner; - + public Participant owner = null; /// /// The network ID of this thing. /// - public byte networkId; + /// @note This field will likely disappear in future versions + public byte networkId = 0; /// /// The ID of this thing /// - public byte id; + public byte id = 0; /// - /// The type of this thing. This can be either a Thing::Type (needs casting) - /// or a byte value for custom types. + /// The type of this thing. /// - public byte type; + /// This can be either a Thing::Type (needs casting) or a byte value for custom types. + public byte type = (byte)Type.Undetermined; - /// - /// Event which is triggered when the parent changes - /// - public event ChangeHandler OnParentChanged = delegate { }; private Thing _parent; /// /// The parent of this thing @@ -137,14 +149,20 @@ namespace RoboidControl { } } } + /// + /// Event which is triggered when the parent changes + /// + public event ChangeHandler OnParentChanged = delegate { }; /// - /// Attach a thing as a child of this thing + /// Add a child Thing to this Thing /// - /// The thing to attach as a child + /// The Thing which should become a child + /// @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); } @@ -152,20 +170,62 @@ namespace RoboidControl { /// Remove the given thing as a child of this thing /// /// The child to remove - public void RemoveChild(Thing child) { - children.Remove(child); + /// True when the child was present or false when it was not found + public bool RemoveChild(Thing child) { + return children.Remove(child); } /// - /// The list of children of this thing + /// Get a child by thing Id /// - [NonSerialized] - public List children = new List(); + /// + /// + /// + Thing GetChild(byte thingId, bool recursively = false) { + foreach (Thing child in this.children) { + if (child == null) + continue; + + if (child.id == thingId) + return child; + if (recursively) { + Thing foundChild = child.GetChild(thingId, recursively); + if (foundChild != null) + return foundChild; + } + } + return null; + } + /// + /// Find a child by name + /// + /// The name of the child thing + /// If true, the name will be searched through descendants recursively + /// The found thing or null when nothing is found + Thing FindChild(string name, bool recursively = true) { + foreach (Thing child in this.children) { + if (child == null) + continue; + + if (child.name == name) + return child; + + if (recursively) { + Thing foundChild = child.FindChild(name, recursively); + if (foundChild != null) + return foundChild; + } + } + return null; + } /// - /// Event which is triggered when the name changes + /// The children of this thing /// - public event ChangeHandler OnNameChanged = delegate { }; + [NonSerialized] + protected List children = new(); + + private string _name = ""; /// /// The name of the thing @@ -179,16 +239,16 @@ namespace RoboidControl { } } } + /// + /// Event which is triggered when the name changes + /// + public event ChangeHandler OnNameChanged = delegate { }; /// /// An URL pointing to the location where a model of the thing can be found /// public string modelUrl = ""; - /// - /// Event triggered when the position has changed - /// - public event ChangeHandler OnPositionChanged = delegate { }; private Spherical _position = Spherical.zero; /// /// The position of the thing in local space, in meters. @@ -198,16 +258,20 @@ namespace RoboidControl { set { if (_position != value) { _position = value; + positionUpdated = true; OnPositionChanged?.Invoke(); } } } - public bool hasPosition = false; - /// - /// Event triggered when the orientation has changed + /// Event triggered when the position has changed /// - public event ChangeHandler OnOrientationChanged = delegate { }; + public event ChangeHandler OnPositionChanged = delegate { }; + /// + /// Boolean indicating that the thing has an updated position + /// + public bool positionUpdated = false; + private SwingTwist _orientation = SwingTwist.zero; /// /// The orientation of the thing in local space @@ -217,15 +281,20 @@ namespace RoboidControl { set { if (_orientation != value) { _orientation = value; + orientationUpdated = true; OnOrientationChanged?.Invoke(); } } } - /// - /// Event triggered when the linear velocity has changed + /// Event triggered when the orientation has changed /// - public event SphericalHandler OnLinearVelocityChanged = delegate { }; + public event ChangeHandler OnOrientationChanged = delegate { }; + /// + /// Boolean indicating the thing has an updated orientation + /// + public bool orientationUpdated = false; + private Spherical _linearVelocity = Spherical.zero; /// /// The linear velocity of the thing in local space in meters per second @@ -235,14 +304,42 @@ namespace RoboidControl { set { if (_linearVelocity != value) { _linearVelocity = value; + linearVelocityUpdated = true; OnLinearVelocityChanged?.Invoke(_linearVelocity); } } } /// - /// The angular velocity of the thing in local space + /// Event triggered when the linear velocity has changed /// - public Spherical angularVelocity = Spherical.zero; + public event SphericalHandler OnLinearVelocityChanged = delegate { }; + /// + /// Boolean indicating the thing has an updated linear velocity + /// + public bool linearVelocityUpdated = false; + + private Spherical _angularVelocity = Spherical.zero; + /// + /// The angular velocity of the thing in local space in degrees per second + /// + public Spherical angularVelocity { + get => _angularVelocity; + set { + if (_angularVelocity != value) { + _angularVelocity = value; + angularVelocityUpdated = true; + OnAngularVelocityChanged?.Invoke(_angularVelocity); + } + } + } + /// + /// Event triggered when the angular velocity has changed + /// + public event SphericalHandler OnAngularVelocityChanged = delegate { }; + /// + /// Boolean indicating the thing has an updated angular velocity + /// + public bool angularVelocityUpdated = false; #if UNITY_5_3_OR_NEWER /// @@ -254,28 +351,28 @@ namespace RoboidControl { #endregion Properties - #region Update + #region Methods - // #if UNITY_5_3_OR_NEWER /// - /// Convience function for use in Unity which removes the need for a currentTime argument + /// Update de state of the thing /// - public void Update(bool recursive = false) { - Update(TimeManager.GetCurrentTimeMilliseconds(), recursive); + /// When true, this will Update the descendants recursively + public void Update(bool recursively = false) { + Update(TimeManager.GetCurrentTimeMilliseconds(), recursively); } // #endif /// /// Update this thing /// /// The current time in milliseconds - public virtual void Update(ulong currentTimeMs, bool recursive = false) { + public virtual void Update(ulong currentTimeMs, bool recursively = false) { // should recurse over children... - if (recursive) { + if (recursively) { for (byte childIx = 0; childIx < this.children.Count; childIx++) { Thing child = this.children[childIx]; if (child == null) continue; - child.Update(currentTimeMs, recursive); + child.Update(currentTimeMs, recursively); } } } @@ -285,7 +382,7 @@ namespace RoboidControl { /// /// a byte array with the binary data /// @sa Passer::RoboidControl::BinaryMsg - public virtual byte[] GenerateBinary() { return new byte[0]; } + public virtual byte[] GenerateBinary() { return Array.Empty(); } /// /// Function used to process binary data received for this thing @@ -294,7 +391,7 @@ namespace RoboidControl { public virtual void ProcessBinary(byte[] bytes) { } - #endregion Update + #endregion Methods /// /// Event triggered when a new thing has been created diff --git a/src/Things/DifferentialDrive.cs b/src/Things/DifferentialDrive.cs index 8dcfb2e..0678ef1 100644 --- a/src/Things/DifferentialDrive.cs +++ b/src/Things/DifferentialDrive.cs @@ -8,7 +8,7 @@ namespace RoboidControl { public DifferentialDrive() { } /// @brief Create a differential drive with networking support /// @param participant The local participant - public DifferentialDrive(LocalParticipant participant) : base(participant, false) { } + public DifferentialDrive(LocalParticipant participant) : base(participant) { } /// @brief Configures the dimensions of the drive /// @param wheelDiameter The diameter of the wheels in meters @@ -28,7 +28,6 @@ namespace RoboidControl { /// Positive moves the robot in the forward direction. /// @param speedRight The speed of the right wheel in degrees per second. /// Positive moves the robot in the forward direction. - public void SetWheelVelocity(float speedLeft, float speedRight) { } /// @copydoc RoboidControl::Thing::Update(unsigned long) diff --git a/src/Things/DistanceSensor.cs b/src/Things/DistanceSensor.cs index c83d940..b47a813 100644 --- a/src/Things/DistanceSensor.cs +++ b/src/Things/DistanceSensor.cs @@ -13,7 +13,7 @@ namespace RoboidControl { /// Constructor for a new distance sensor /// /// The participant for which the sensor is needed - public DistanceSensor(Participant participant) : base(participant, true) { } + public DistanceSensor(Participant participant) : base(participant) { } /// /// Create a distance sensor with the given ID /// diff --git a/src/Things/TouchSensor.cs b/src/Things/TouchSensor.cs index 22dc290..e4f0e05 100644 --- a/src/Things/TouchSensor.cs +++ b/src/Things/TouchSensor.cs @@ -12,7 +12,7 @@ namespace RoboidControl { /// /// The participant for with the sensor is needed /// True when the creation should trigger an event - public TouchSensor(Participant owner, bool invokeEvent = true) : base(owner, invokeEvent) { + public TouchSensor(Participant owner) : base(owner) { //touchedSomething = false; //thisParticipant = owner; } diff --git a/src/TimeManger.cs b/src/TimeManager.cs similarity index 83% rename from src/TimeManger.cs rename to src/TimeManager.cs index 875f459..bb00724 100644 --- a/src/TimeManger.cs +++ b/src/TimeManager.cs @@ -1,6 +1,9 @@ using System.Diagnostics; namespace RoboidControl { + /// + /// Time manager is een tool mainly to get the current running time in milliseconds + /// public static class TimeManager { private static readonly Stopwatch _stopwatch = new Stopwatch(); diff --git a/test/UnitTest1.cs b/test/UnitTest1.cs index 27012a9..c8c129e 100644 --- a/test/UnitTest1.cs +++ b/test/UnitTest1.cs @@ -65,7 +65,7 @@ namespace RoboidControl.test { public void Test_ThingMsg() { SiteServer siteServer = new SiteServer(7681); LocalParticipant participant = new LocalParticipant("127.0.0.1"); - Thing thing = new Thing(participant, false) { + Thing thing = new Thing(participant) { name = "First Thing", modelUrl = "https://passer.life/extras/ant.jpg" };