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