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

Improvements, alignmet with Cpp/Python

Browse Source
This commit is contained in:
Pascal Serrarens 2025-03-11 17:35:03 +01:00
parent 2c498a6dd9
commit 608b40fa86
11 changed files with 228 additions and 106 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
Examples/BB2B/Program.cs
View File

@ -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);
}

8
RoboidControl-csharp.code-workspace Normal file
View File

@ -0,0 +1,8 @@
{
"folders": [
{
"path": "."
}
],
"settings": {}
}

9
src/LocalParticipant.cs
View File

@ -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)

68
src/Participant.cs
View File

@ -4,29 +4,19 @@ using System.Collections.Generic;
namespace RoboidControl {
/// <summary>
/// A reference to a participant, possibly on a remote location
/// A participant is a device which manages things.
/// </summary>
/// 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 {
/// <summary>
/// The internet address of the participant
/// </summary>
public string ipAddress = "0.0.0.0";
/// <summary>
/// The UDP port on which the participant can be reached
/// </summary>
public int port = 0;
/// <summary>
/// The network ID of the participant
/// </summary>
public byte networkId;
/// <summary>
/// Default constructor
/// </summary>
public Participant() { }
/// <summary>
/// Create a new remote participant
/// Create a new participant with the given communcation info
/// </summary>
/// <param name="ipAddress">The IP address of the participant</param>
/// <param name="port">The UDP port of the participant</param>
@ -36,7 +26,21 @@ namespace RoboidControl {
}
/// <summary>
/// The things reported by this participant
/// The Ip Address of a participant. When the participant is local, this contains 0.0.0.0
/// </summary>
public string ipAddress = "0.0.0.0";
/// <summary>
/// The port number for UDP communication with the participant. This is 0 for isolated participants.
/// </summary>
public int port = 0;
/// <summary>
/// The network ID of the participant
/// </summary>
public byte networkId;
/// <summary>
/// The things managed by this participant
/// </summary>
protected readonly List<Thing> things = new List<Thing>();
@ -58,7 +62,11 @@ namespace RoboidControl {
/// </summary>
/// <param name="thing">The thing to add</param>
/// <param name="invokeEvent">Invoke an notification event when the thing has been added</param>
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}]");
// }
// }
}
/// <summary>
/// Remove a thing for this participant
/// </summary>
/// <param name="thing">The thing to remove</param>
public void Remove(Thing thing) {
this.things.Remove(thing);
}
}

2
src/SiteServer.cs
View File

@ -73,7 +73,7 @@ namespace RoboidControl {
newThing.parent = parentThing;
}
Console.WriteLine("Adding to remote sender");
sender.Add(newThing);
sender.Add(newThing, false, true);
}
}

217
src/Thing.cs
View File

@ -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
/// <summary>
/// Create a new thing without communication abilities
/// </summary>
/// <param name="thingType">The type of thing</param>
public Thing(byte thingType = (byte)Type.Undetermined) : this(LocalParticipant.Isolated(), thingType) {
}
/// <summary>
/// Create a new thing for a participant
/// </summary>
/// <param name="owner">The participant owning the thing</param>
/// <param name="thingType">The type of thing</param>
public Thing(Participant owner, byte thingType = (byte)Type.Undetermined) {
this.owner = owner;
this.type = thingType;
}
/// <summary>
/// Create a new thing as a child of another thing
/// </summary>
/// <param name="parent">The parent thing</param>
/// <param name="thingType">The type of thing</param>
public Thing(Thing parent, byte thingType = (byte)Type.Undetermined) : this(parent.owner, thingType) {
this.parent = parent;
}
/*
/// <summary>
/// Create a new thing for the given participant
/// </summary>
/// <param name="owner">The participant for which this thing is created</param>
/// <param name="invokeEvent">True when a new thing event should be triggered</param>
public Thing(Participant owner, bool invokeEvent = false) {
this.owner = owner;
if (invokeEvent)
InvokeNewThing(this);
}
*/
/// <summary>
/// Create a new thing for the given participant
/// </summary>
/// <param name="owner">The participant for which this thing is created</param>
/// <param name="invokeEvent">True when a new thing event should be triggered</param>
public Thing(Participant owner, bool invokeEvent = false) {
this.owner = owner;
//owner.Add(this);
if (invokeEvent)
InvokeNewThing(this);
}
/// <summary>
/// Create a new thing for the given participant
/// </summary>
/// <param name="participant">The participant for which this thing is created</param>
/// <param name="participant">The participant owning the thing</param>
/// <param name="networkId">The network ID of the thing</param>
/// <param name="thingId">The ID of the thing</param>
/// <param name="thingType">The type of thing</param>
@ -89,34 +109,26 @@ namespace RoboidControl {
#region Properties
public delegate void ChangeHandler();
public delegate void SphericalHandler(Spherical v);
public delegate void ThingHandler(Thing t);
/// <summary>
/// The participant to which this thing belongs
/// The participant owning this thing
/// </summary>
public Participant owner;
public Participant owner = null;
/// <summary>
/// The network ID of this thing.
/// </summary>
public byte networkId;
/// @note This field will likely disappear in future versions
public byte networkId = 0;
/// <summary>
/// The ID of this thing
/// </summary>
public byte id;
public byte id = 0;
/// <summary>
/// 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.
/// </summary>
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;
/// <summary>
/// Event which is triggered when the parent changes
/// </summary>
public event ChangeHandler OnParentChanged = delegate { };
private Thing _parent;
/// <summary>
/// The parent of this thing
@ -137,14 +149,20 @@ namespace RoboidControl {
}
}
}
/// <summary>
/// Event which is triggered when the parent changes
/// </summary>
public event ChangeHandler OnParentChanged = delegate { };
/// <summary>
/// Attach a thing as a child of this thing
/// Add a child Thing to this Thing
/// </summary>
/// <param name="child">The thing to attach as a child</param>
/// <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);
}
@ -152,20 +170,62 @@ namespace RoboidControl {
/// Remove the given thing as a child of this thing
/// </summary>
/// <param name="child">The child to remove</param>
public void RemoveChild(Thing child) {
children.Remove(child);
/// <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>
/// The list of children of this thing
/// Get a child by thing Id
/// </summary>
[NonSerialized]
public List<Thing> children = new List<Thing>();
/// <param name="thingId"></param>
/// <param name="recursively"></param>
/// <returns></returns>
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;
}
/// <summary>
/// Find a child by name
/// </summary>
/// <param name="name">The name of the child thing</param>
/// <param name="recursively">If true, the name will be searched through descendants recursively</param>
/// <returns>The found thing or null when nothing is found</returns>
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;
}
/// <summary>
/// Event which is triggered when the name changes
/// The children of this thing
/// </summary>
public event ChangeHandler OnNameChanged = delegate { };
[NonSerialized]
protected List<Thing> children = new();
private string _name = "";
/// <summary>
/// The name of the thing
@ -179,16 +239,16 @@ namespace RoboidControl {
}
}
}
/// <summary>
/// Event which is triggered when the name changes
/// </summary>
public event ChangeHandler OnNameChanged = delegate { };
/// <summary>
/// An URL pointing to the location where a model of the thing can be found
/// </summary>
public string modelUrl = "";
/// <summary>
/// Event triggered when the position has changed
/// </summary>
public event ChangeHandler OnPositionChanged = delegate { };
private Spherical _position = Spherical.zero;
/// <summary>
/// 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;
/// <summary>
/// Event triggered when the orientation has changed
/// Event triggered when the position has changed
/// </summary>
public event ChangeHandler OnOrientationChanged = delegate { };
public event ChangeHandler OnPositionChanged = delegate { };
/// <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
@ -217,15 +281,20 @@ namespace RoboidControl {
set {
if (_orientation != value) {
_orientation = value;
orientationUpdated = true;
OnOrientationChanged?.Invoke();
}
}
}
/// <summary>
/// Event triggered when the linear velocity has changed
/// Event triggered when the orientation has changed
/// </summary>
public event SphericalHandler OnLinearVelocityChanged = delegate { };
public event ChangeHandler OnOrientationChanged = delegate { };
/// <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
@ -235,14 +304,42 @@ namespace RoboidControl {
set {
if (_linearVelocity != value) {
_linearVelocity = value;
linearVelocityUpdated = true;
OnLinearVelocityChanged?.Invoke(_linearVelocity);
}
}
}
/// <summary>
/// The angular velocity of the thing in local space
/// Event triggered when the linear velocity has changed
/// </summary>
public Spherical angularVelocity = Spherical.zero;
public event SphericalHandler OnLinearVelocityChanged = delegate { };
/// <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;
OnAngularVelocityChanged?.Invoke(_angularVelocity);
}
}
}
/// <summary>
/// Event triggered when the angular velocity has changed
/// </summary>
public event SphericalHandler OnAngularVelocityChanged = delegate { };
/// <summary>
/// Boolean indicating the thing has an updated angular velocity
/// </summary>
public bool angularVelocityUpdated = false;
#if UNITY_5_3_OR_NEWER
/// <summary>
@ -254,28 +351,28 @@ namespace RoboidControl {
#endregion Properties
#region Update
#region Methods
// #if UNITY_5_3_OR_NEWER
/// <summary>
/// Convience function for use in Unity which removes the need for a currentTime argument
/// Update de state of the thing
/// </summary>
public void Update(bool recursive = false) {
Update(TimeManager.GetCurrentTimeMilliseconds(), recursive);
/// <param name="recursively">When true, this will Update the descendants recursively</param>
public void Update(bool recursively = false) {
Update(TimeManager.GetCurrentTimeMilliseconds(), recursively);
}
// #endif
/// <summary>
/// Update this thing
/// </summary>
/// <param name="currentTime">The current time in milliseconds</param>
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 {
/// </summary>
/// <returns>a byte array with the binary data</returns>
/// @sa Passer::RoboidControl::BinaryMsg
public virtual byte[] GenerateBinary() { return new byte[0]; }
public virtual byte[] GenerateBinary() { return Array.Empty<byte>(); }
/// <summary>
/// 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
/// <summary>
/// Event triggered when a new thing has been created

3
src/Things/DifferentialDrive.cs
View File

@ -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)

2
src/Things/DistanceSensor.cs
View File

@ -13,7 +13,7 @@ namespace RoboidControl {
/// Constructor for a new distance sensor
/// </summary>
/// <param name="participant">The participant for which the sensor is needed</param>
public DistanceSensor(Participant participant) : base(participant, true) { }
public DistanceSensor(Participant participant) : base(participant) { }
/// <summary>
/// Create a distance sensor with the given ID
/// </summary>

2
src/Things/TouchSensor.cs
View File

@ -12,7 +12,7 @@ namespace RoboidControl {
/// </summary>
/// <param name="owner">The participant for with the sensor is needed</param>
/// <param name="invokeEvent">True when the creation should trigger an event</param>
public TouchSensor(Participant owner, bool invokeEvent = true) : base(owner, invokeEvent) {
public TouchSensor(Participant owner) : base(owner) {
//touchedSomething = false;
//thisParticipant = owner;
}

3
src/TimeManger.cs → src/TimeManager.cs
View File

@ -1,6 +1,9 @@
using System.Diagnostics;
namespace RoboidControl {
/// <summary>
/// Time manager is een tool mainly to get the current running time in milliseconds
/// </summary>
public static class TimeManager {
private static readonly Stopwatch _stopwatch = new Stopwatch();

2
test/UnitTest1.cs
View File

@ -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"
};