/// <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;
}
}
}
// Display message from robot
void DisplayInboundMessage(EgmRobot robot)
{
if (robot.HasHeader && robot.Header.HasSeqno && robot.Header.HasTm)
{
Console.WriteLine("Seq={0} tm={1}", robot.Header.Seqno.ToString(), robot.Header.Tm.ToString());
}
else
{
Console.WriteLine("No header in robot message");
}
}
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;
}
}
}
}
}
private void DoRecvEgm(UdpClient egm_socket, ref IPEndPoint ep)
{
var recv = egm_socket.Receive(ref ep);
egm_robot = new EgmRobot();
egm_robot.MergeFrom(new CodedInputStream(recv));
lock (this)
{
if (egm_robot.HasFeedBack && egm_robot.FeedBack.HasJoints && egm_robot.FeedBack.HasCartesian && egm_robot.HasMotorState && egm_robot.HasRapidExecState)
{
last_recv = now_func();
var joint_msg = egm_robot.FeedBack.Joints.Joints;
if (actual_joint_position == null || actual_joint_position.Length != joint_msg.Count)
{
actual_joint_position = new double[joint_msg.Count];
}
for (int i = 0; i < joint_msg.Count; i++)
{
actual_joint_position[i] = joint_msg[i] * (Math.PI / 180.0);
}
var cart = egm_robot.FeedBack.Cartesian;
tcp_pose.position.x = cart.Pos.X / 1000.0;
tcp_pose.position.y = cart.Pos.Y / 1000.0;
tcp_pose.position.z = cart.Pos.Z / 1000.0;
tcp_pose.orientation.w = cart.Orient.U0;
tcp_pose.orientation.x = cart.Orient.U1;
tcp_pose.orientation.y = cart.Orient.U2;
tcp_pose.orientation.z = cart.Orient.U3;
enabled = egm_robot.HasMotorState;
ready = egm_robot.RapidExecState.State == EgmRapidCtrlExecState.Types.RapidCtrlExecStateType.RapidRunning;
}
}
}
// Display message from robot
void DisplayInboundMessage(EgmRobot robot)
{
if (robot.HasHeader && robot.Header.HasSeqno && robot.Header.HasTm)
{
Console.WriteLine("Seq={0} tm={1}",
robot.Header.Seqno.ToString(), robot.Header.Tm.ToString());
}
else
{
Console.WriteLine("No header in robot message");
}
}
