protected void sendState(SmartComponent component)
{
int PORT = (int)component.Properties["PortNumber"].Value;
using (var sock = new UdpClient())
{
LineSensor.Builder sensorData = LineSensor.CreateBuilder();
Point.Builder sensedPoint = new Point.Builder();
Point.Builder start = new Point.Builder();
Point.Builder end = new Point.Builder();
UInt32 sensorIDProperty = Convert.ToUInt32(component.Properties["SensorID"].Value);
Vector3 sensedPointProperty = (Vector3)component.Properties["SensedPoint"].Value;
String sensedPartProperty = Convert.ToString(component.Properties["SensedPart"].Value);
Vector3 startProperty = (Vector3)component.Properties["Start"].Value;
Vector3 endProperty = (Vector3)component.Properties["End"].Value;
double radiusProperty = (double)component.Properties["Radius"].Value;
// convert point from m to mm
sensedPointProperty = sensedPointProperty.Multiply(1000);
sensedPoint.SetX(sensedPointProperty.x)
.SetY(sensedPointProperty.y)
.SetZ(sensedPointProperty.z);
start.SetX(startProperty.x)
.SetY(startProperty.y)
.SetZ(startProperty.z);
end.SetX(endProperty.x)
.SetY(endProperty.y)
.SetZ(endProperty.z);
sensorData.SetSensedPoint(sensedPoint)
.SetStart(start)
.SetEnd(end)
.SetRadius(radiusProperty)
.SetSensedPart(sensedPartProperty)
.SetSensorID(sensorIDProperty);
//if(sensorData.SensorID == 42)
//{
// Debug.WriteLine($"sensor nbr {sensorData.SensorID} numbers: ({sensorData.SensedPoint.X}, {sensorData.SensedPoint.Y}, {sensorData.SensedPoint.Z})");
//}
LineSensor data = sensorData.Build();
using (MemoryStream memoryStream = new MemoryStream())
{
data.WriteTo(memoryStream);
var bytesSent = sock.SendAsync(memoryStream.ToArray(), (int)memoryStream.Length, "localhost", PORT);
//int bytesSent = sock.Send(memoryStream.ToArray(), (int)memoryStream.Length, "localhost", PORT);
//if (bytesSent < 0)
//{
// Console.WriteLine("ERROR");
//}
}
//byte[] bytes = Encoding.UTF8.GetBytes("hello, world!");
//sock.Send(bytes, bytes.Length, "localhost", PORT);
}
}
/// <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;
}
}
void CheckLineSensors()
{
Debug.Log("CheckLineSensors()");
// Clear our line sensor data
List <LineSensor> LineFoundBy = new List <LineSensor>();
foreach (LineSensor sensor in Controller.LineSensors)
{
if (sensor.Hit)
{
LineFoundBy.Add(sensor);
}
}
this.LineFoundBy = LineFoundBy.ToArray();
// What direction is the line?
LineDirection = "Test";
float sumX = 0.0f;
float sumZ = 0.0f;
// float avgX, avgZ;
//foreach ( LineSensor sensor in LineFoundBy ) {
for (int i = 0; i < this.LineFoundBy.Length; i++)
{
LineSensor sensor = this.LineFoundBy[i];
Vector3 relativePoint = transform.InverseTransformPoint(sensor.transform.position);
sumX += (float)System.Math.Round(relativePoint.x, 3);
sumZ += (float)System.Math.Round(relativePoint.z, 3);
}
AvgX = sumX / LineFoundBy.Count;
AvgZ = sumZ / LineFoundBy.Count;
if (AvgZ > 0)
{
LineDirection = "North";
}
else if (AvgZ < 0)
{
LineDirection = "South";
}
else
{
LineDirection = "Middle";
}
if (AvgX > 0)
{
LineDirection += " East";
}
else if (AvgX < 0)
{
LineDirection += " West";
}
else
{
LineDirection += " Center";
}
}
public RobotTestBase()
{
map = new Map(100, 100);
env = new DefaultEnvironment(map);
var defaultRobot = new RobotBase(env);
lineSensor = new LineSensor(defaultRobot);
robot = defaultRobot as IRobot;
distanceSensor = new DistanceSensor(defaultRobot);
robot.Location = new Point(50.0, 50.0);
robot.Orientation = 0.0;
}
