void MonteCarlo_UpdatePosition(Object state)
{
ts_position_t position = new ts_position_t();
position.theta = (float)(actualPosition.angle * 180 / Math.PI);
position.x = 1000 * actualPosition.x + SLAMAlgorithm.TS_MAP_SIZE / (SLAMAlgorithm.TS_MAP_SCALE * 2);
position.y = 1000 * actualPosition.y + SLAMAlgorithm.TS_MAP_SIZE / (SLAMAlgorithm.TS_MAP_SCALE * 2);
ts_scan_t scan = this.doScan();
int quality;
ts_position_t newPos = tinySLAM.MonteCarlo_UpdatePosition(scan, position, 1, 1, 10000, 50, out quality);
Position p = new Position();
p.x = (newPos.x - SLAMAlgorithm.TS_MAP_SIZE / (SLAMAlgorithm.TS_MAP_SCALE * 2)) * 1e-3;
p.y = (newPos.y - SLAMAlgorithm.TS_MAP_SIZE / (SLAMAlgorithm.TS_MAP_SCALE * 2)) * 1e-3;
p.angle = newPos.theta * Math.PI / 180;
Position diff = p - actualPosition;
//Only the new position will be sent if is near of the actual position
if (diff.x < 3e-2 && diff.y < 3e-2 && diff.angle < 5 * Math.PI / 180)
{
coder.Send(Message.UpdatePosition, p);
}
}
public void Scan(object state)
{
UpdaterInt d = new UpdaterInt(updateIterationsLabel);
this.Dispatcher.Invoke(d, tinySLAM.NumUpdates());
ts_scan_t scan = this.doScan();
ts_position_t position = new ts_position_t();
position.theta = (float)(actualPosition.angle * 180 / Math.PI);
position.x = 1000 * actualPosition.x + SLAMAlgorithm.TS_MAP_SIZE / (SLAMAlgorithm.TS_MAP_SCALE * 2);
position.y = 1000 * actualPosition.y + SLAMAlgorithm.TS_MAP_SIZE / (SLAMAlgorithm.TS_MAP_SCALE * 2);
tinySLAM.NewScan(scan, position);
}
private ts_scan_t doScan()
{
ts_scan_t scan = new ts_scan_t();
int j = 0;
int angle = 0;
//CENTRAL SENSOR:
if (lastCentral > 0)
{
angle -= 10;
for (int i = 0; i < 20; angle++, i++, j++)
{
double angleRad = (float)(angle * Math.PI / 180);
double c = Math.Cos(angleRad);
double s = Math.Sin(angleRad);
scan.x[j] = (float)(lastCentral * s);
scan.y[j] = (float)(lastCentral * c + 230 / 2) * (-1);
scan.value[j] = SLAMAlgorithm.TS_OBSTACLE;
}
}
else if (lastCentral == -1)
{
angle -= 10;
for (int i = 0; i < 20; angle++, i++, j++)
{
double angleRad = (float)(angle * Math.PI / 180);
double c = Math.Cos(angleRad);
double s = Math.Sin(angleRad);
scan.x[j] = (float)(800 * s);
scan.y[j] = (float)(800 * c + 230 / 2) * (-1);
scan.value[j] = SLAMAlgorithm.TS_NO_OBSTACLE;
}
}
//WALL SENSOR
if (lastWall > 0)
{
angle = 0;
angle -= 10;
for (int i = 0; i < 20; angle++, i++, j++)
{
double angleRad = (angle * Math.PI / 180);
double c = Math.Cos(angleRad);
double s = Math.Sin(angleRad);
scan.x[j] = (float)(lastWall * c + 185 / 2) * (-1);
scan.y[j] = (float)(230 / 2 + lastWall * s) * (-1);
scan.value[j] = SLAMAlgorithm.TS_OBSTACLE;
}
}
else if (lastWall == -1)
{
angle = 0;
angle -= 10;
for (int i = 0; i < 20; angle++, i++, j++)
{
double angleRad = (angle * Math.PI / 180);
double c = Math.Cos(angleRad);
double s = Math.Sin(angleRad);
scan.x[j] = (float)(300 * c + 185 / 2) * (-1);
scan.y[j] = (float)(230 / 2 + 300 * s) * (-1);
scan.value[j] = SLAMAlgorithm.TS_NO_OBSTACLE;
}
}
//WALLBACK SENSOR
if (lastWallBack > 0)
{
angle = 0;
angle -= 10;
for (int i = 0; i < 20; angle++, i++, j++)
{
double angleRad = (angle * Math.PI / 180);
double c = Math.Cos(angleRad);
double s = Math.Sin(angleRad);
scan.x[j] = (float)(lastWallBack * c + 185 / 2) * (-1);
scan.y[j] = (float)(lastWallBack * s - 230 / 2) * (-1);
scan.value[j] = SLAMAlgorithm.TS_OBSTACLE;
}
}
else if (lastWallBack == -1)
{
angle = 0;
angle -= 10;
for (int i = 0; i < 20; angle++, i++, j++)
{
double angleRad = (angle * Math.PI / 180);
double c = Math.Cos(angleRad);
double s = Math.Sin(angleRad);
scan.x[j] = (float)(300 * c + 185 / 2) * (-1);
scan.y[j] = (float)(300 * s - 230 / 2) * (-1);
scan.value[j] = SLAMAlgorithm.TS_NO_OBSTACLE;
}
}
//RIGHT SENSOR
if (lastRight > 0)
{
angle = 0;
angle -= 10;
for (int i = 0; i < 20; angle++, i++, j++)
{
double angleRad = (angle * Math.PI / 180);
double rad45 = (45 * Math.PI / 180);
double c = Math.Cos(angleRad + rad45);
double s = Math.Sin(angleRad + rad45);
scan.x[j] = (float)(lastRight * c + 185 / 2);
scan.y[j] = (float)(lastRight * s + 230 / 2) * (-1);
scan.value[j] = SLAMAlgorithm.TS_OBSTACLE;
}
}
else if (lastRight == -1)
{
angle = 0;
angle -= 10;
for (int i = 0; i < 20; angle++, i++, j++)
{
double angleRad = (angle * Math.PI / 180);
double rad45 = (45 * Math.PI / 180);
double c = Math.Cos(angleRad + rad45);
double s = Math.Sin(angleRad + rad45);
scan.x[j] = (float)(800 * c + 185 / 2);
scan.y[j] = (float)(800 * s + 230 / 2) * (-1);
scan.value[j] = SLAMAlgorithm.TS_NO_OBSTACLE;
}
}
scan.nb_points = (ushort)j;
return(scan);
}
