private Stream GenerateTop(int imageWidth)
{
SonarDataDssSerializable lsd = _state.MostRecentSonar;
if (lsd == null)
{
return(null);
}
int nRays = lsd.RangeMeters.Length;
MemoryStream memory = null;
Font font = new Font(FontFamily.GenericSansSerif, 10, GraphicsUnit.Pixel);
// Ultrasonic sensor reaches to about 3.5 meters; we scale the height of our display to this range:
double maxExpectedRange = 5000.0d; // mm
int imageHeight = imageWidth / 2;
int extraHeight = 100; // shows state of proximity sensors
using (Bitmap bmp = new Bitmap(imageWidth, imageHeight + extraHeight))
{
using (Graphics g = Graphics.FromImage(bmp))
{
g.Clear(Color.LightGray);
double angularOffset = -90 + 180.0d / 2.0d;
double piBy180 = Math.PI / 180.0d;
double halfAngle = 1.0d / 2.0d;
double scale = imageHeight / maxExpectedRange;
double drangeMax = 0.0d;
GraphicsPath path = new GraphicsPath();
Dictionary <int, Lbl> labels = new Dictionary <int, Lbl>();
for (int pass = 0; pass != 2; pass++)
{
for (int i = 0; i < nRays; i++)
{
int range = (int)(lsd.RangeMeters[i] * 1000.0d);
if (range > 0 && range < 8192)
{
double angle = i * _state.AngularResolution - angularOffset;
double lowAngle = (angle - halfAngle) * piBy180;
double highAngle = (angle + halfAngle) * piBy180;
double drange = range * scale;
float lx = (float)(imageHeight + drange * Math.Cos(lowAngle));
float ly = (float)(imageHeight - drange * Math.Sin(lowAngle));
float hx = (float)(imageHeight + drange * Math.Cos(highAngle));
float hy = (float)(imageHeight - drange * Math.Sin(highAngle));
if (pass == 0)
{
if (i == 0)
{
path.AddLine(imageHeight, imageHeight, lx, ly);
}
path.AddLine(lx, ly, hx, hy);
drangeMax = Math.Max(drangeMax, drange);
}
else
{
g.DrawLine(Pens.DarkBlue, lx, ly, hx, hy);
if (i > 0 && i % 2 == 0 && i < nRays - 1)
{
float llx = (float)(imageHeight + drangeMax * 1.3f * Math.Cos(lowAngle));
float lly = (float)(imageHeight - drangeMax * 1.3f * Math.Sin(lowAngle));
double roundRange = Math.Round(range / 1000.0d, 1); // meters
string str = "" + roundRange;
labels.Add(i, new Lbl()
{
label = str, lx = llx, ly = lly
});
}
}
}
}
if (pass == 0)
{
g.FillPath(Brushes.White, path);
}
}
// now draw the robot. Trackroamer is 680 mm wide.
float botHalfWidth = (float)(680 / 2.0d * scale);
DrawHelper.drawRobotBoundaries(g, botHalfWidth, imageWidth / 2, imageHeight);
g.DrawString(lsd.TimeStamp.ToString(), font, Brushes.Black, 0, 0);
foreach (int x in labels.Keys)
{
Lbl lbl = labels[x];
g.DrawString(lbl.label, font, Brushes.Black, lbl.lx, lbl.ly);
}
// draw a 200x400 image of IR proximity sensors:
Rectangle drawRect = new Rectangle(imageWidth / 2 - extraHeight, imageHeight - extraHeight * 2, extraHeight * 2, extraHeight * 4);
if (_state.MostRecentProximity != null)
{
DrawHelper.drawProximityVectors(g, drawRect, _state.MostRecentProximity.arrangedForDrawing, 1);
}
if (_state.MostRecentParkingSensor != null)
{
DrawHelper.drawProximityVectors(g, drawRect, _state.MostRecentParkingSensor.arrangedForDrawing, 2);
}
}
memory = new MemoryStream();
bmp.Save(memory, ImageFormat.Jpeg);
memory.Position = 0;
}
return(memory);
}
private Stream GenerateCylinder()
{
SonarDataDssSerializable lsd = _state.MostRecentSonar;
if (lsd == null)
{
return(null);
}
int nRays = lsd.RangeMeters.Length;
MemoryStream memory = null;
int scalefactor = 23; // 600 / 26 = 23.0769231 - to have width 600px when nRays = 26
int bmpWidth = nRays * scalefactor;
int nearRange = 300;
int farRange = 2000;
int bmpHeight = 100;
int topTextHeight = 17; // leave top for the text
Font font = new Font(FontFamily.GenericSansSerif, 10, GraphicsUnit.Pixel);
using (Bitmap bmp = new Bitmap(bmpWidth, bmpHeight))
{
using (Graphics g = Graphics.FromImage(bmp))
{
g.Clear(Color.LightGray);
int half = (int)Math.Round(bmp.Height * 0.65d);
int middle = nRays / 2;
int rangeMax = 0;
int pointMax = -1;
Dictionary <int, string> labels = new Dictionary <int, string>();
for (int i = 0; i < nRays; i++)
{
int range = (int)(lsd.RangeMeters[i] * 1000.0d);
int x = i * scalefactor;
if (i == middle)
{
g.DrawLine(Pens.Gray, x, topTextHeight, x, bmpHeight);
}
if (range > 0 && range < 8192)
{
if (range > rangeMax)
{
rangeMax = range;
pointMax = x;
}
int h = bmp.Height * 300 / range;
if (h < 0)
{
h = 0;
}
Color col = ColorHelper.LinearColor(Color.OrangeRed, Color.LightGreen, nearRange, farRange, range);
//Color col = ColorHelper.LinearColor(Color.DarkBlue, Color.LightGray, 0, 8192, range);
g.DrawLine(new Pen(col, (float)scalefactor), bmp.Width - x, Math.Max(topTextHeight, half - h), bmp.Width - x, Math.Min(bmpHeight, half + h));
if (i > 0 && i % 2 == 0 && i < nRays - 1)
{
double roundRange = Math.Round(range / 1000.0d, 1); // meters
string str = "" + roundRange;
labels.Add(x, str);
}
}
}
foreach (int x in labels.Keys)
{
// draw labels (distance of every second ray in meters):
string str = labels[x];
g.DrawString(str, font, Brushes.Black, bmp.Width - x - 8, (int)(bmpHeight - topTextHeight * 2 + Math.Abs((double)middle - x / scalefactor) * 20 / middle) - 30);
}
if (pointMax > 0)
{
// draw a vertical green line where the distance reaches its max value:
double roundRangeMax = Math.Round(rangeMax / 1000.0d, 1); // meters
int lineWidth = 4;
g.DrawLine(new Pen(Color.DarkGreen, (float)lineWidth), bmp.Width - pointMax - lineWidth / 2, half, bmp.Width - pointMax - lineWidth / 2, bmpHeight);
g.DrawString("" + roundRangeMax + "m", font, Brushes.DarkGreen, bmp.Width - pointMax, bmpHeight - topTextHeight);
}
g.DrawString(
lsd.TimeStamp.ToString() + " max: " + rangeMax + " red: <" + nearRange + " green: >" + farRange + " mm",
font, Brushes.Black, 0, 0
);
}
memory = new MemoryStream();
bmp.Save(memory, ImageFormat.Jpeg);
memory.Position = 0;
}
return(memory);
}
