public void MessageRoundTrip()
{
List<IMotion> motions = new List<IMotion>
{
new LinearMotion(1, new Vector(5,10),new Vector(3,5.5)),
new CircularMotion(30, 1, new Angle(0), new Angle(0), 1, Vector.Zero),
new LinearMotion(500, new Vector(7,20),new Vector(5,100)),
new CircularMotion(670, 0, new Angle(0), new Angle(-Math.PI), 1, Vector.Zero),
new CircularMotion(1500, 0, new Angle(-Math.PI / 2), new Angle(1), 1, Vector.Zero)
};
const string name = "bobby";
var original = new PathMessage(name, motions);
var serialiser = new MessageSerialiser();
serialiser.Register(typeof(PathMessage));
var text = serialiser.Serialise(original);
var deserialised = (PathMessage)serialiser.Deserialise(text);
Assert.AreEqual(name, deserialised.Name);
Assert.AreEqual(motions, deserialised.Motion.Path);
}
public void MessageRoundTrip()
{
List <IMotion> motions = new List <IMotion>
{
new LinearMotion(1, new Vector(5, 10), new Vector(3, 5.5)),
new CircularMotion(30, 1, new Angle(0), new Angle(0), 1, Vector.Zero),
new LinearMotion(500, new Vector(7, 20), new Vector(5, 100)),
new CircularMotion(670, 0, new Angle(0), new Angle(-Math.PI), 1, Vector.Zero),
new CircularMotion(1500, 0, new Angle(-Math.PI / 2), new Angle(1), 1, Vector.Zero)
};
const string name = "bobby";
var original = new PathMessage(name, motions);
var serialiser = new MessageSerialiser();
serialiser.Register(typeof(PathMessage));
var text = serialiser.Serialise(original);
var deserialised = (PathMessage)serialiser.Deserialise(text);
Assert.AreEqual(name, deserialised.Name);
Assert.AreEqual(motions, deserialised.Motion.Path);
}
private void UpdateMotion(PathMessage message)
{
foreach (var node in m_SceneNodes)
{
if (node.PathId == message.Name)
{
node.Motion = message.Motion;
}
}
}
static void Main(string[] args)
{
// display usage info
Console.WriteLine("Testing Vehicle Telemetry Application");
Console.WriteLine("The program will now predefined data to the telemetry application over TCP.");
Console.WriteLine("Press CTRL+C to cancel.\n");
// register control-c handler
Console.CancelKeyPress += delegate(object sender, ConsoleCancelEventArgs e) {
e.Cancel = true;
Program.isRunning = false;
};
// start communication
Console.WriteLine("Connecting to 127.0.0.1 : 5640");
isRunning = true;
try {
MessageClientTcp client = new MessageClientTcp();
using (client) {
// connect to server
client.Connect("127.0.0.1", 5640);
// send layout
LayoutMessage layoutMsg = new LayoutMessage();
layoutMsg.Count = 3;
layoutMsg[0].Dimension = 4;
layoutMsg[0].Name = "GPS";
layoutMsg[0].Id = 1;
layoutMsg[0].AppendPathId = 10;
layoutMsg[0].AppendPathEnabled = true;
layoutMsg[0].UpdateMap = true;
layoutMsg[0][0] = "Latitude";
layoutMsg[0][1] = "Longitude";
layoutMsg[0][2] = "Altitude";
layoutMsg[0][3] = "Speed (kph)";
layoutMsg[1].Dimension = 3;
layoutMsg[1].Name = "Speed";
layoutMsg[1].Id = 2;
layoutMsg[1].AppendPathId = 0;
layoutMsg[1].AppendPathEnabled = false;
layoutMsg[1].UpdateMap = false;
layoutMsg[1][0] = "Forward";
layoutMsg[1][1] = "Right";
layoutMsg[1][2] = "Upward";
layoutMsg[2].Dimension = 4;
layoutMsg[2].Name = "Motors";
layoutMsg[2].Id = 3;
layoutMsg[2].AppendPathId = 0;
layoutMsg[2].AppendPathEnabled = false;
layoutMsg[2].UpdateMap = false;
layoutMsg[2][0] = "Motor #1";
layoutMsg[2][1] = "Motor #2";
layoutMsg[2][2] = "Motor #3";
layoutMsg[2][3] = "Motor #4";
Console.WriteLine("Sending layout...");
client.Send(layoutMsg);
PathMessage pathMsg = new PathMessage();
pathMsg.action = PathMessage.eAction.ADD_PATH;
pathMsg.path = 10;
pathMsg.name = "Trajectory";
client.Send(pathMsg);
Console.WriteLine("Configuring paths...");
Console.WriteLine();
// keep sending some data periodically
long iteration = 1;
GeoPoint currentPoint = new GeoPoint(47.5, 19, 130);
GeoPoint lastPoint = currentPoint;
double elapsedTotal = 0;
double frameTime = 1;
Stopwatch timer = new Stopwatch();
timer.Start();
while (isRunning)
{
Console.WriteLine("updating data... #" + iteration.ToString());
Console.SetCursorPosition(0, Console.CursorTop - 1);
iteration++;
DataMessage dataMsg = new DataMessage();
// gps
currentPoint = new GeoPoint(
47.5 + 0.05 * Math.Sin(0.01 * elapsedTotal),
19 + 0.1 * Math.Cos(0.01 * elapsedTotal),
130);
dataMsg.Id = 1;
dataMsg.Dimension = 4;
dataMsg[0] = (float)currentPoint.Lat;
dataMsg[1] = (float)currentPoint.Lng;
dataMsg[2] = (float)130;
dataMsg[3] = (float)(GeoPoint.DistanceDirect(currentPoint, lastPoint) / frameTime * 3.6);
lastPoint = currentPoint;
client.Send(dataMsg);
// speed
dataMsg.Id = 2;
dataMsg.Dimension = 3;
dataMsg[0] = 0.0f;
dataMsg[1] = 0.0f;
dataMsg[2] = 0.0f;
client.Send(dataMsg);
// motors
dataMsg.Id = 3;
dataMsg.Dimension = 4;
dataMsg[0] = (float)RandomGauss(40, 1.5);
dataMsg[1] = (float)RandomGauss(40, 2.5);
dataMsg[2] = (float)RandomGauss(40, 1.3);
dataMsg[3] = (float)RandomGauss(40, 1.8);
client.Send(dataMsg);
// wait a little
Thread.Sleep(200);
double elapsedTotalNew = timer.Elapsed.TotalSeconds;
frameTime = elapsedTotalNew - elapsedTotal;
elapsedTotal = elapsedTotalNew;
}
// close connection
Console.WriteLine("Disconnecting...");
client.Disconnect();
}
}
catch (Exception e) {
Console.WriteLine("An error occured: " + e.Message + "\n");
}
}
