// Create a sensor message to send to the robot
void CreateSensorMessage(EgmSensor.Builder sensor)
{
// create a header
EgmHeader.Builder hdr = new EgmHeader.Builder();
hdr.SetSeqno(_seqNumber++)
.SetTm((uint)DateTime.Now.Ticks) // Timestamp in milliseconds (can be used for monitoring delays)
.SetMtype(EgmHeader.Types.MessageType.MSGTYPE_CORRECTION); // Sent by sensor, MSGTYPE_DATA if sent from robot controller
sensor.SetHeader(hdr);
// create some sensor data
EgmPlanned.Builder planned = new EgmPlanned.Builder();
EgmPose.Builder pos = new EgmPose.Builder();
EgmQuaternion.Builder pq = new EgmQuaternion.Builder();
EgmCartesian.Builder pc = new EgmCartesian.Builder();
pc.SetX(X)
.SetY(Y)
.SetZ(Z);
pq.SetU0(0.0)
.SetU1(0.0)
.SetU2(0.0)
.SetU3(0.0);
pos.SetPos(pc)
.SetOrient(pq);
planned.SetCartesian(pos); // bind pos object to planned
sensor.SetPlanned(planned); // bind planned to sensor object
}
//////////////////////////////////////////////////////////////////////////
// Create a sensor message to send to the robot
void CreateSensorMessage(EgmSensor.Builder sensor)
{
// create a header
EgmHeader.Builder hdr = new EgmHeader.Builder();
hdr.SetSeqno(_seqNumber++)
.SetTm((uint)DateTime.Now.Ticks) // Timestamp in milliseconds (can be used for monitoring delays)
.SetMtype(EgmHeader.Types.MessageType.MSGTYPE_CORRECTION); // Sent by sensor, MSGTYPE_DATA if sent from robot controller
sensor.SetHeader(hdr);
// create some sensor data
EgmPlanned.Builder planned = new EgmPlanned.Builder();
EgmPose.Builder pos = new EgmPose.Builder();
EgmQuaternion.Builder pq = new EgmQuaternion.Builder();
EgmCartesian.Builder pc = new EgmCartesian.Builder();
// Dividing X and Y by two to avoid sending targets outside the robots reach
pc.SetX(Convert.ToInt32(this.X / 2))
.SetY(0)
.SetZ(Convert.ToInt32(this.Z / 2));
pq.SetU0(0.0)
.SetU1(0.0)
.SetU2(0.0)
.SetU3(0.0);
pos.SetPos(pc)
.SetOrient(pq);
planned.SetCartesian(pos); // bind pos object to planned
sensor.SetPlanned(planned); // bind planned to sensor object
return;
}
// Create a sensor message to send to the robot
void CreateSensorMessage(EgmSensor.Builder sensor)
{
// create a header
EgmHeader.Builder hdr = new EgmHeader.Builder();
hdr.SetSeqno(_seqNumber++).SetTm((uint)DateTime.Now.Ticks).SetMtype(EgmHeader.Types.MessageType.MSGTYPE_CORRECTION);
// Timestamp in milliseconds , sent by sensor, MSGTYPE_DATA if sent from robot controller
sensor.SetHeader(hdr);
// create some sensor data
EgmPlanned.Builder planned = new EgmPlanned.Builder();
EgmPose.Builder pos = new EgmPose.Builder();
EgmQuaternion.Builder pq = new EgmQuaternion.Builder();
EgmCartesian.Builder pc = new EgmCartesian.Builder();
EgmJoints.Builder pj = new EgmJoints.Builder();
pj.Build();
// use this to set systematic changes to coordinates
pj.AddJoints(1).AddJoints(2).AddJoints(3);
pj.SetJoints(0, this.axis1);
pj.SetJoints(1, this.axis2);
pj.SetJoints(2, this.axis3);
planned.SetJoints(pj); // bind joints object to planned
sensor.SetPlanned(planned); // bind planned to sensor obj
// display sensor data
DisplaySensorMessage(this.axis1, this.axis2, this.axis3);
return;
}
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;
}
}
}
}
}
//////////////////////////////////////////////////////////////////////////
// Create a sensor message to send to the robot
void CreateSensorMessage(EgmSensor.Builder sensor)
{
// create a header
EgmHeader.Builder hdr = new EgmHeader.Builder();
hdr.SetSeqno(_seqNumber++)
.SetTm((uint)DateTime.Now.Ticks)
.SetMtype(EgmHeader.Types.MessageType.MSGTYPE_CORRECTION);
sensor.SetHeader(hdr);
// create some sensor data
EgmPlanned.Builder planned = new EgmPlanned.Builder();
EgmPose.Builder pos = new EgmPose.Builder();
EgmQuaternion.Builder pq = new EgmQuaternion.Builder();
EgmCartesian.Builder pc = new EgmCartesian.Builder();
//pc.SetX(1200)
// .SetY(11.1)
// .SetZ(1000);
pc.SetX(0)
.SetY(0)
.SetZ(-10);
pq.SetU0(0.32557)
.SetU1(0.0)
.SetU2(0.94552)
.SetU3(0.0);
pos.SetPos(pc)
.SetOrient(pq);
planned.SetCartesian(pos); // bind pos object to planned
sensor.SetPlanned(planned); // bind planned to sensor object
Console.WriteLine("CreateSensorMessage");
log.Debug(sensor);
return;
}
