//////////////////////////////////////////////////////////////////////////
// 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) // 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
}
/// <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);
}
//////////////////////////////////////////////////////////////////////////
// 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;
}
//////////////////////////////////////////////////////////////////////////
// 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++)
//Timestamp in milliseconds (can be used for monitoring delays)
.SetTm((uint)DateTime.Now.Ticks)
//Sent by sensor, MSGTYPE_DATA if sent from robot controller
.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();
_x = X_Values_Square();
_y = Y_Values_Square();
pc.SetX(_x)
.SetY(_y)
.SetZ(Height);
pq.SetU0(0.0)
.SetU1(0.0)
.SetU2(0.0)
.SetU3(0.0);
pos.SetPos(pc)
.SetOrient(pq);
// bind pos object to planned
planned.SetCartesian(pos);
// bind planned to sensor object
sensor.SetPlanned(planned);
return;
}