public void SensorThread() { // create an udp client and listen on any address and the port IpPortNumber _udpServer = new UdpClient(Program.IpPortNumber); var remoteEp = new IPEndPoint(IPAddress.Any, Program.IpPortNumber); _stopwatch.Start(); while (ExitThread == false) { //SimDiscSetPos(); CameraSetPos(); // Get the message from robot var data = _udpServer.Receive(ref remoteEp); if (data != null) { Console.WriteLine(_predictor.PredictedPosition.ToString()); // de-serialize inbound message from robot using Google Protocol Buffer EgmRobot robot = EgmRobot.CreateBuilder().MergeFrom(data).Build(); XRobot = robot.FeedBack.Cartesian.Pos.X; YRobot = robot.FeedBack.Cartesian.Pos.Y; ZRobot = robot.FeedBack.Cartesian.Pos.Z; // create a new outbound sensor message EgmSensor.Builder sensor = EgmSensor.CreateBuilder(); CreateSensorMessage(sensor); XSensor = sensor.Planned.Cartesian.Pos.X; YSensor = sensor.Planned.Cartesian.Pos.Y; ZSensor = sensor.Planned.Cartesian.Pos.Z; using (MemoryStream memoryStream = new MemoryStream()) { EgmSensor sensorMessage = sensor.Build(); sensorMessage.WriteTo(memoryStream); // send the udp message to the robot int bytesSent = _udpServer.Send(memoryStream.ToArray(), (int)memoryStream.Length, remoteEp); if (bytesSent < 0) { Console.WriteLine("Error send to robot"); } } SavePositionToFile(); _prevXSensor = XSensor; _prevYSensor = YSensor; _prevZSensor = ZSensor; } } }
/// <summary> /// If the EgmUdpThread on port DemoEgmPortNumbers.POS_GUIDE_PORT calls read: /// then build a Google Protocol Buffer message of type EgmSensor and calculate /// the next position required to follow the head. /// Default, return null because there is no other EgmUdpThread than needs to send /// data back to its client. /// </summary> /// <param name="udpPortNbr"></param> /// <returns></returns> public byte[] Read(int udpPortNbr) { byte[] data; switch (udpPortNbr) { case (int)DemoEgmPortNumbers.POS_GUIDE_PORT: // builder for an EgmSensor message EgmSensor.Builder sensor = EgmSensor.CreateBuilder(); // builder for the header EgmHeader.Builder hdr = new EgmHeader.Builder(); // data for the header hdr.SetSeqno((uint)seqNbr++) .SetTm((uint)DateTime.Now.Ticks) .SetMtype(EgmHeader.Types.MessageType.MSGTYPE_CORRECTION); // set the data into the header sensor.SetHeader(hdr); // create some builders for the body of the EgmSensor message EgmPlanned.Builder planned = new EgmPlanned.Builder(); EgmPose.Builder pos = new EgmPose.Builder(); EgmQuaternion.Builder pq = new EgmQuaternion.Builder(); EgmCartesian.Builder pc = new EgmCartesian.Builder(); // calculate the next Y position to send to the robot controller // i.e. current position + ((sensed position + offset) - current position)*(some overshot for control) double nextY = feedback[1] + ((sensedPoint[1] + offset) - feedback[1]) * 1.6; // set the data pc.SetX(922.868225097656) .SetY(nextY) .SetZ(1407.03857421875); pq.SetU0(1.0) .SetU1(0.0) .SetU2(0.0) .SetU3(0.0); pos.SetPos(pc) .SetOrient(pq); planned.SetCartesian(pos); sensor.SetPlanned(planned); EgmSensor sensorMessage = sensor.Build(); using (MemoryStream memoryStream = new MemoryStream()) { sensorMessage.WriteTo(memoryStream); data = memoryStream.ToArray(); } break; default: Debug.WriteLine($"No defined Read() case for data going to port {udpPortNbr}."); data = null; break; } return(data); }
public void SensorThread() { try { // create an udp client and listen on any address and the port IpPortNumber // allow sockets to be reused for local network transmissions // needed for simulations var remoteEp = new IPEndPoint(IPAddress.Any, Program.IpPortNumber); _udpServer = new System.Net.Sockets.UdpClient(); _udpServer.ExclusiveAddressUse = false; _udpServer.Client.SetSocketOption(SocketOptionLevel.Socket, SocketOptionName.ReuseAddress, true); _udpServer.Client.Bind(remoteEp); while (_exitThread == false) { // Get message from robot var data = _udpServer.Receive(ref remoteEp); if (data != null) { // de-serialize inbound message from robot using Google Protocol Buffer EgmRobot robot = EgmRobot.CreateBuilder().MergeFrom(data).Build(); // display inbound message DisplayInboundMessage(robot); // create a new outbound sensor message EgmSensor.Builder sensor = EgmSensor.CreateBuilder(); CreateSensorMessage(sensor); using (MemoryStream memoryStream = new MemoryStream()) { EgmSensor sensorMessage = sensor.Build(); sensorMessage.WriteTo(memoryStream); // send the udp message to the robot int bytesSent = _udpServer.Send(memoryStream.ToArray(), (int)memoryStream.Length, remoteEp); if (bytesSent < 0) { Console.WriteLine("Error send to robot"); } } } } } catch (Exception e) { Console.WriteLine("Error in receive Sockets : " + e.Message); Console.WriteLine(e.Data); Console.WriteLine(e.HResult); Console.WriteLine(e.Source); Console.WriteLine(e.StackTrace); } }
public void SensorThread() { // create an udp client and listen on any address and the port _ipPortNumber _udpServer = new UdpClient(Program.IpPortNumber); var remoteEP = new IPEndPoint(IPAddress.Any, Program.IpPortNumber); while (_exitThread == false) { // get the message from robot var data = _udpServer.Receive(ref remoteEP); if (data != null) { // de-serialize inbound message from robot using Google Protocol Buffer EgmRobot robot = EgmRobot.CreateBuilder().MergeFrom(data).Build(); // display inbound message DisplayInboundMessage(robot); // Get the robots X-position _robotX = Convert.ToInt32((robot.FeedBack.Cartesian.Pos.X)); // Get the robots Y-position _robotY = Convert.ToInt32((robot.FeedBack.Cartesian.Pos.Y)); // Get the robots Z-position _robotZ = Convert.ToInt32((robot.FeedBack.Cartesian.Pos.Z)); // create a new outbound sensor message EgmSensor.Builder sensor = EgmSensor.CreateBuilder(); CreateSensorMessage(sensor); using (MemoryStream memoryStream = new MemoryStream()) { EgmSensor sensorMessage = sensor.Build(); sensorMessage.WriteTo(memoryStream); // send the UDP message to the robot int bytesSent = _udpServer.Send(memoryStream.ToArray(), (int)memoryStream.Length, remoteEP); if (bytesSent < 0) { Console.WriteLine("Error send to robot"); } } } } }
public void SensorThread() { // create an udp client and listen on any address and the port _ipPortNumber _udpServer = new UdpClient(EGM._ipPortNumber); var remoteEP = new IPEndPoint(IPAddress.Any, EGM._ipPortNumber); while (_exitThread == false) { // get the message from robot var data = _udpServer.Receive(ref remoteEP); if (data != null) { // de-serialize inbound message from robot EgmRobot robot = EgmRobot.CreateBuilder().MergeFrom(data).Build(); // display inbound message DisplayInboundMessage(robot); // create a new outbound sensor message EgmSensor.Builder sensor = EgmSensor.CreateBuilder(); CreateSensorMessage(sensor); using (MemoryStream memoryStream = new MemoryStream()) { EgmSensor sensorMessage = sensor.Build(); sensorMessage.WriteTo(memoryStream); if (boolFlag) { // send the udp message to the robot int bytesSent = _udpServer.Send(memoryStream.ToArray(), (int)memoryStream.Length, remoteEP); if (bytesSent < 0) { Console.WriteLine("Error send to robot"); } else { Console.WriteLine("data send to robot"); } //boolFlag = false; } } } } }