public void ReceiverThread()
{
IPEndPoint remote = new IPEndPoint(IPAddress.Any, UDP_PORT);
byte[] data = null;
while (runReceiverThread)
{
try
{
data = udpSocket.Receive(ref remote);
if (remote.Port != UDP_PORT)
{
continue;
}
}
catch (System.Net.Sockets.SocketException e)
{
continue;
//socket closed, finish execution if run is false
}
if (data.Length != Marshal.SizeOf(frames[0]))
{
MessageBox.Show(".Net frame=" + Marshal.SizeOf(frames[0]).ToString() + " received=" + data.Length.ToString());
continue;
}
GCHandle handle = GCHandle.Alloc(data, GCHandleType.Pinned);
RobotFrame frame = new RobotFrame();
try
{
IntPtr ptr = handle.AddrOfPinnedObject();
frame = (RobotFrame)Marshal.PtrToStructure(ptr, typeof(RobotFrame));
}
finally
{
handle.Free();
}
laserCanvas.Dispatcher.BeginInvoke(new RefresherHandler(RefreshReadings), null, frame);
}
}
public void RefreshReadings(object sender, RobotFrame frame)
{
map_point_to_update = 0;
const double deg2rad = Math.PI / 180.0;
double centerx = laserCanvas.ActualWidth / 2.0;
double centery = laserCanvas.ActualHeight / 2.0;
double middlex = centerx - POINT_RADIUS;
double middley = centery - POINT_RADIUS;
laserRPMTextBlock.Text = frame.laser_speed.ToString();
double packet_time_sec = (double)frame.elapsed_ms / 100000.0;
const double ENCODER_SCALE_FACTOR = Math.PI * 4.32 / 360.0;
double ldiff = frame.left_stop - frame.left_start;
double rdiff = frame.right_stop - frame.right_start;
double displacement_cm = (ldiff + rdiff) * ENCODER_SCALE_FACTOR / 2.0;
speedBuffer.Add(displacement_cm / packet_time_sec);
speedTextBlock.Text = (-speedBuffer.Average).ToString("F2");
double gyro_start = frame.angle_start / 100.0;
double gyro_stop = frame.angle_stop / 100.0;
double angle_difference = (gyro_stop - gyro_start);
if (Math.Abs(angle_difference) > 180.0)
{
angle_difference = Math.Sign(angle_difference) * 360.0 - angle_difference;
}
angleTextBlock.Text = gyro_stop.ToString();
double gyro_rad = gyro_stop * deg2rad;
double gyro_average_rad = (gyro_start + angle_difference / 2.0) * deg2rad;
double max_mapping_distance = 10000;
if (maxMappingDistanceDoubleUpDown.Value.HasValue)
{
max_mapping_distance = maxMappingDistanceDoubleUpDown.Value.Value;
}
int point_index;
LaserReading[] readings = frame.laser_readings;
for (int i = 0; i < readings.Length; ++i)
{
point_index = i + frame.laser_angle * 4;
if (readings[i].unknown_warning == 1)
{
Console.Out.WriteLine("Uknown warning flag triggered! ");
}
if (readings[i].invalid_data == 1)
{
points[point_index].Visibility = Visibility.Hidden;
continue;//readings[i].distance = 160;
}
else
{
points[point_index].Visibility = Visibility.Visible;
}
points[point_index].Stroke = DeterminePointColor(readings[i]);
double angle = point_index;
double angle_offset = gyro_start + i * angle_difference / (readings.Length - 1);
if (absoluteRotationCheckBox.IsChecked == true)
{
angle -= angle_offset;
}
double rad = deg2rad * angle;
double y, x;
if (correctGeometryCheckBox.IsChecked == false)
{
y = -Math.Cos(rad) * readings[i].distance / 10.0;
x = -Math.Sin(rad) * readings[i].distance / 10.0;
}
else //(correctGeometryCheckBox.IsChecked == true)
{
double yp = -Math.Cos(rad) * readings[i].distance;
double xp = -Math.Sin(rad) * readings[i].distance;
double alpha = 180 - 82 + angle;
double alpharad = deg2rad * alpha;
double b = 25;
x = (xp + b * Math.Sin(alpharad)) / 10;
y = (yp + b * Math.Cos(alpharad)) / 10;
}
if (absolutePositionCheckBox.IsChecked == true)
{
double angle_offset_rad = deg2rad * angle_offset;
double displacement_offset = displacement_cm * (double)i / (double)(readings.Length - 1);
x += positionEstimate.X - displacement_offset * Math.Sin(gyro_average_rad);
y += positionEstimate.Y + displacement_offset * Math.Cos(gyro_average_rad);
if (mapEstimateCheckBox.IsChecked == true && ((double)readings[i].distance / 10.0) < max_mapping_distance)
{
map_update[map_point_to_update].X = x + map_center_x;
map_update[map_point_to_update].Y = y + map_center_y;
++map_point_to_update;
}
}
Canvas.SetLeft(points[point_index], x + middlex + viewOffset.X);
Canvas.SetTop(points[point_index], y + middley + viewOffset.Y);
}
RefreshPath(displacement_cm, gyro_average_rad, gyro_stop, centerx, centery);
RefreshPointer(centerx, centery, gyro_rad, speedBuffer.Average);
if (mapEstimateCheckBox.IsChecked == true && map_point_to_update > 0)
{
if (mapMovingCheckBox.IsChecked == true || displacement_cm == 0 && gyro_start == gyro_stop)
{
UpdateMap();
}
}
}
