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