/// <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; } } }