/// <summary>
/// Data from DemoEgmPortNumbers.POS_GUIDE_PORT is a Google Protocol Buffer message of type EgmRobot.
/// Data from DemoEgmPortNumbers.LINE_SENSOR_PORT is a Google Protocol Buffer message of type LineSensor
/// Data from any other port is unknown and not handeled by this monitor.
/// </summary>
/// <param name="udpPortNbr"></param>
/// <param name="data"></param>
public void Write(int udpPortNbr, byte[] data)
{
switch (udpPortNbr)
{
case (int)DemoEgmPortNumbers.POS_GUIDE_PORT:
EgmRobot robot = EgmRobot.CreateBuilder().MergeFrom(data).Build();
feedback = new double[] {
robot.FeedBack.Cartesian.Pos.X,
robot.FeedBack.Cartesian.Pos.Y,
robot.FeedBack.Cartesian.Pos.Z
};
break;
case (int)DemoEgmPortNumbers.LINE_SENSOR_PORT:
LineSensor state = LineSensor.CreateBuilder().MergeFrom(data).Build();
if (state.SensorID == 42)
{
sensedPoint = new double[]
{
state.SensedPoint.X,
state.SensedPoint.Y,
state.SensedPoint.Z
};
}
break;
default:
Debug.WriteLine($"No defined Write() case for data coming from port {udpPortNbr}.");
break;
}
}
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;
}
}
}
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;
}
}
}
}
}
