/// <summary>
/// Reset everything
/// </summary>
public void Reset()
{
Array.Fill(HoleMap.Pixels, (ushort)((TS_OBSTACLE + TS_NO_OBSTACLE) / 2));
ArrayEx.Fill(ObstacleMap.Pixels, UnmappedObstacleHits);
Pose = startPose;
lastOdometryPose = Vector3.Zero;
scanCount = 0;
}
/// <summary>
/// Update obstacle map with lidar scan points cloud
/// </summary>
/// <param name="cloud">Scan points cloud</param>
private void UpdateObstacleMap(ScanCloud cloud)
{
ArrayEx.Fill(noHitMap, false);
// Pre-calculate centerpoint with additional 0.5 to get rounding effect
float px = Pose.X * ObstacleMap.Scale + 0.5f;
float py = Pose.Y * ObstacleMap.Scale + 0.5f;
float c = MathF.Cos(Pose.Z) * ObstacleMap.Scale;
float s = MathF.Sin(Pose.Z) * ObstacleMap.Scale;
// TODO Use cloud lines to draw map ?
// Line start
int x1 = (int)px;
int y1 = (int)py;
// Boundary check
if (x1 < 0 || x1 >= ObstacleMap.Size || y1 < 0 || y1 >= ObstacleMap.Size)
{
return; // Robot is out of map
}
// Draw rays
foreach (Vector2 p in cloud.Points)
{
// Translate and rotate scan to robot position
int x2 = (int)(px + c * p.X - s * p.Y);
int y2 = (int)(py + s * p.X + c * p.Y);
// Draw obstacle map
DrawLaserRayOnObstacleMap(noHitMap, x1, y1, x2, y2);
}
// TODO Far small objects disappear too easily... do some tricks
// Deal with no hit pixels
for (int y = 0; y < ObstacleMap.Size; y++)
{
for (int x = 0; x < ObstacleMap.Size; x++)
{
if (noHitMap[y, x])
{
if (ObstacleMap.Pixels[y, x] < 0)
{
ObstacleMap.Pixels[y, x]++;
}
else if (ObstacleMap.Pixels[y, x] > 0)
{
ObstacleMap.Pixels[y, x]--;
}
}
}
}
}
