private void UpdateMagnifyCtrl(Point pos)
{
var yCoord = pos.Y >= MagnifyHalfWidth ? pos.Y - MagnifyHalfWidth : pos.Y - (MagnifyHalfWidth - (MagnifyHalfWidth - pos.Y));
var xCoord = pos.X >= MagnifyHalfWidth ? pos.X - MagnifyHalfWidth : pos.X - (MagnifyHalfWidth - (MagnifyHalfWidth - pos.X));
var rect = new Int32Rect((int)xCoord, (int)yCoord, MagnifyWidth, MagnifyWidth);
MagnifyCtrl.ImageSource = ImgProc.CropImageSource(_screenSnapshot.BitmapImage, rect, 10);
}
private void RefreshDimensions()
{
bmpRaw = new Bitmap((int)Math.Round((int)NumBoxScale.Value * RATIO), (int)NumBoxScale.Value, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
bmpProc = new Bitmap((int)Math.Round((int)NumBoxScale.Value * RATIO), (int)NumBoxScale.Value, System.Drawing.Imaging.PixelFormat.Format1bppIndexed);
bounds = new Rectangle((int)NumBoxX.Value, (int)NumBoxY.Value, (int)Math.Round((int)NumBoxScale.Value * RATIO), (int)NumBoxScale.Value);
screenToBMP();
ImgRaw.Refresh();
ImgProc.Refresh();
}
public void SetData(IplImage image)
{
var bufferHandle = GCHandle.Alloc(textureBuffer, GCHandleType.Pinned);
var bufferHeader = new IplImage(image.Size, 8, 4, bufferHandle.AddrOfPinnedObject());
ImgProc.cvCvtColor(image, bufferHeader, ColorConversion.BGR2RGBA);
bufferHandle.Free();
update = true;
}
void CaptureNewFrame()
{
while (running)
{
using (var depthFrame = kinect.DepthStream.OpenNextFrame(50))
using (var colorFrame = kinect.ColorStream.OpenNextFrame(50))
using (var skeletonFrame = kinect.SkeletonStream.OpenNextFrame(50))
{
if (depthFrame != null && colorFrame != null && skeletonFrame != null)
{
// Keep depth as managed array
var depthImageBuffer = new short[depthFrame.PixelDataLength];
//if (depthImageBuffer == null || depthImageBuffer.Length != depthFrame.PixelDataLength)
//{
// depthImageBuffer = new short[depthFrame.PixelDataLength];
//}
if (colorImageBuffer == null || colorImageBuffer.Length != colorFrame.PixelDataLength)
{
colorImageBuffer = new byte[colorFrame.PixelDataLength];
}
var skeletonData = new Skeleton[skeletonFrame.SkeletonArrayLength];
skeletonFrame.CopySkeletonDataTo(skeletonData);
colorFrame.CopyPixelDataTo(colorImageBuffer);
depthFrame.CopyPixelDataTo(depthImageBuffer);
// Flip color image to get proper measurements
var bufferHandle = GCHandle.Alloc(colorImageBuffer, GCHandleType.Pinned);
var colorImage = new IplImage(new CvSize(colorFrame.Width, colorFrame.Height), colorFrame.BytesPerPixel * 8 / 4, 4, bufferHandle.AddrOfPinnedObject());
var colorOutput = new IplImage(colorImage.Size, colorImage.Depth, 3);
ImgProc.cvCvtColor(colorImage, colorOutput, ColorConversion.BGRA2BGR);
Core.cvFlip(colorOutput, colorOutput, FlipMode.Horizontal);
bufferHandle.Free();
//bufferHandle = GCHandle.Alloc(depthImageBuffer, GCHandleType.Pinned);
//var depthImage = new IplImage(new CvSize(depthFrame.Width, depthFrame.Height), depthFrame.BytesPerPixel * 8, 1, bufferHandle.AddrOfPinnedObject());
//var depthOutput = new IplImage(depthImage.Size, depthImage.Depth, 1);
//Core.cvCopy(depthImage, depthOutput);
//Core.cvFlip(depthOutput, depthOutput, FlipMode.Horizontal);
//bufferHandle.Free();
//**** Flip depth horizontally ****
//bufferHandle = GCHandle.Alloc(depthImageBuffer, GCHandleType.Pinned);
//var depthImage = new IplImage(new CvSize(depthFrame.Width, depthFrame.Height), depthFrame.BytesPerPixel * 8, 1, bufferHandle.AddrOfPinnedObject());
//Core.cvFlip(depthImage, depthImage, FlipMode.Horizontal);
//bufferHandle.Free();
Subject.OnNext(new KinectFrame(kinect, depthImageBuffer, colorOutput, skeletonData));
}
}
}
stop.Set();
}
/// <summary>
/// Закончено выделение области на канвасах
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void DrawCanvasOnMouseUp(object sender, MouseButtonEventArgs e)
{
if (e.LeftButton == MouseButtonState.Released)
{
SquareAreaSelect.Visibility = Visibility.Collapsed;
if (ScreenAreaCaptured != null)
{
var display = ImgProc.CropImageSource(_screenSnapshot.BitmapImage, _currentRectangle);
display.Freeze();
ScreenAreaCaptured.Invoke(display);
}
}
}
public void AddNewCaseToDb()
{
var manualProcPath = ImgProc.GetManualProcessPath();
var testAccessionFilePath = Path.Combine(manualProcPath, "TestAccession");
if (Directory.GetFiles(manualProcPath).Length > 0)
{
Assert.Fail($"Manual Processing directory [{manualProcPath}] has to be empty!");
}
File.Create(testAccessionFilePath).Close();
var allAccessions = Directory.GetFiles(manualProcPath).Select(Path.GetFileName).ToArray();
if (allAccessions.Length != 1)
{
Assert.Fail($"Test file was not created in [{manualProcPath}] directory.");
}
var accession = allAccessions.FirstOrDefault();
const int topEntriesCount = 1000;
var accessionsInDb = _agentConsoleRepository.GetRecentVerifiedMris(topEntriesCount);
var accessionAlreadyInDb = accessionsInDb.Select(a => a.Accession).Contains(accession);
if (accessionAlreadyInDb)
{
Assert.Fail($"Accession [{accession}] already exists in DB. Please remove it and try agian.");
}
var verifiedMri = _agentConsoleFactory.CreateVerifiedMri();
verifiedMri.Accession = accession;
verifiedMri.AdditionMethod = "Unit Testing";
verifiedMri.Status = "Testing";
verifiedMri.AdditionTime = DateTime.Now;
_agentConsoleRepository.InsertVerifiedMriIntoDb(verifiedMri);
accessionsInDb = _agentConsoleRepository.GetRecentVerifiedMris(topEntriesCount);
var accessionExistsInDb = accessionsInDb.Select(a => a.Accession).Contains(accession);
if (!accessionExistsInDb)
{
Assert.Fail($"Accession [{accession}] does not exist in DB.");
}
}
public void Cleanup()
{
var manualProcPath = ImgProc.GetManualProcessPath();
var testAccessionFilePath = Path.Combine(manualProcPath, "TestAccession");
if (File.Exists(testAccessionFilePath))
{
File.Delete(testAccessionFilePath);
}
// Delete any case in DB with accession 'TestAccession'
var testAccessionCase = _agentConsoleFactory.CreateVerifiedMri();
testAccessionCase.Accession = "TestAccession";
if (_agentConsoleRepository.AccessionExistsInDb(testAccessionCase.Accession))
{
_agentConsoleRepository.DeleteInDbByAccession(testAccessionCase.Accession);
}
}
private void MainLoop(object sender, DoWorkEventArgs e)
{
while (!(sender as BackgroundWorker).CancellationPending)
{
screenToBMP();
if (processBmp())
{
Invoke(new Action(() =>
{
StatusStrip.BackColor = System.Drawing.Color.FromArgb(255, 180, 0, 0);
StatusLabel.Text = (int.Parse(StatusLabel.Text) + 1).ToString();
}));
UpdateLights();
Invoke(new Action(() => StatusStrip.BackColor = System.Drawing.Color.FromArgb(255, 48, 48, 48)));
}
Invoke(new Action(() =>
{
ImgRaw.Refresh();
ImgProc.Refresh();
}));
}
}
/// <summary>
/// Worker thread for image processing.
/// </summary>
public void CvMainThread()
{
var faceCascade = new CascadeClassifier();
var eyesCascade = new CascadeClassifier();
faceCascade.load("haarcascade_frontalface_alt.xml");
eyesCascade.load("haarcascade_eye_tree_eyeglasses.xml");
var srcFrame = new Mat();
var dstFrame = new Mat();
var imgProc = new ImgProc();
_videoIo.StartCapture();
while (true)
{
_videoIo.GetFrame(srcFrame);
switch (_processingMethodIndex)
{
// passthrough
case 0:
break;
// gray
case 1:
imgProc.cvtColor(srcFrame, dstFrame, ColorConversionCodes.COLOR_RGBA2GRAY);
imgProc.cvtColor(dstFrame, srcFrame, ColorConversionCodes.COLOR_GRAY2RGB);
break;
// canny
case 3:
imgProc.cvtColor(srcFrame, dstFrame, cvRT.ColorConversionCodes.COLOR_RGBA2GRAY);
imgProc.GaussianBlur(dstFrame, dstFrame, new cvRT.Size(7, 7), 1.5, 1.5);
imgProc.Canny(dstFrame, dstFrame, 0, 30, 3);
imgProc.cvtColor(dstFrame, srcFrame, ColorConversionCodes.COLOR_GRAY2RGB);
break;
// contour
case 4:
{
var contours = new VectorOfVectorOfPoint();
var hierarchy = new VectorOfVec4i();
var color = new Scalar(255, 255, 255, 255);
imgProc.Canny(srcFrame, dstFrame, 100, 100 * 2, 3);
imgProc.FindContours(dstFrame, contours, hierarchy, ContourRetrievalAlgorithm.RETR_TREE, ContourApproximationModes.CHAIN_APPROX_SIMPLE, new Point(0, 0));
srcFrame.Set(new Scalar(0, 0, 0, 0));
for (var i = 0 ; i < contours.Count(); i++)
{
imgProc.DrawContours(srcFrame, contours, i, color, 2, 8, hierarchy, 0, new Point(0, 0));
}
break;
}
// face detect
case 5:
{
imgProc.cvtColor(srcFrame, dstFrame, ColorConversionCodes.COLOR_RGBA2GRAY);
imgProc.EqualizeHist(dstFrame, dstFrame);
// Faces in the frame.
var faces = new List<Rect>();
try
{
faces = new List<Rect>();
faceCascade.detectMultiScale(dstFrame, faces, 1.1, 2, (int)(0 | CV_HAAR.SCALE_IMAGE), new cvRT.Size(30, 30));
}
catch (Exception ex)
{
Debug.WriteLine("Exception {0}", ex.Message);
}
// For each face, detect the eyes
foreach (var face in faces)
{
// Draw ellipse for the face.
var faceCenter = new Point(face.X + face.Width / 2, face.Y + face.Height / 2);
imgProc.Ellipse(srcFrame, faceCenter, new cvRT.Size(face.Width / 2, face.Height / 2), 0, 0, 360, new Scalar(255, 0, 255, 0), 4, 8, 0);
// Detect the eyes for the face
var faceRoi = dstFrame.RectOfInterest(face);
var eyes = new List<Rect>();
eyesCascade.detectMultiScale(faceRoi, eyes, 1.1, 2, (int) (0 | CASCADE_FLAG.CASCADE_SCALE_IMAGE),new cvRT.Size(30, 30));
// Draw the eyes
foreach (var eye in eyes)
{
var eyeCenter = new Point(face.X + eye.X + eye.Width/2, face.Y + eye.Y + eye.Height/2);
var radius = (int) Math.Round((eye.Width + eye.Height) * 0.25);
imgProc.Circle(srcFrame, eyeCenter, radius, new Scalar(255, 0, 0, 0), 4, 8, 0);
}
}
break;
}
default:
break;
}
_videoIo.ShowFrame(srcFrame);
}
}
private void CreateCSVBtn_Click(object sender, EventArgs e)
{
//部分变量暂时是通过写死的方式；
Button btn = sender as Button;
try
{
int Threshold1 = 40;
int Threshold2 = 100;
int Threshold3 = 50;
string FilePath = "";
string Sufix = "";
btn.Enabled = false;
eRadErr RadRes = eRadErr.OK;
if (this.murasholdbox1.Text == "" || this.murasholdbox2.Text == "")
{
MessageBox.Show("请设置MuraThreshold参数！");
return;
}
if (SelectHighPass == null)
{
MessageBox.Show("请选择高通滤波数据生成方式！");
return;
}
float mThreshold1 = Convert.ToSingle(this.murasholdbox1.Text);
float mThreshold2 = Convert.ToSingle(this.murasholdbox2.Text);
List <Task> Taskls = new List <Task>();
FolderBrowserDialog folder = new FolderBrowserDialog();
folder.Description = "选择所有文件存放的目录";
if (folder.ShowDialog() == DialogResult.OK)
{
FilePath = folder.SelectedPath;
}
foreach (Pattern patinfo in SelectModelRecipe.PatternArray)
{
string ImgPath = "";
double t1 = 0, t2 = 0, t3 = 0;
float muraNum = 0;
switch (patinfo.Color)
{
case "R":
ImgPath = $"{FilePath}\\Red{patinfo.Gary}.bmp";
break;
case "B":
ImgPath = $"{FilePath}\\blue{patinfo.Gary}.bmp";
break;
case "G":
ImgPath = $"{FilePath}\\Green{patinfo.Gary}.bmp";
break;
default:
break;
}
if (!File.Exists(ImgPath))
{
MessageBox.Show(ImgPath);
return;
}
StringBuilder SbImgPath = new StringBuilder(ImgPath);
StringBuilder SbCsvPath = new StringBuilder(ImgPath.Replace(".bmp", ".csv").Trim());
Taskls.Add(Task.Factory.StartNew(() =>
{
int Res = ImgProc.ImageProc(SbImgPath, SbCsvPath, patinfo.Size, Threshold1, Threshold2, Threshold3,
patinfo.Threshold4, SelectModelRecipe.Width, SelectModelRecipe.Height,
patinfo.OffsetH, patinfo.OffsetW, patinfo.Scale, true, true, true, ref t1,
ref t2, ref t3, 100, ref muraNum, mThreshold1, mThreshold2);
string ResMsg = ImgProc.GetErrorInfo((ImgProErrorCode)Res);
if (Res == (int)ImgProErrorCode.ImageError_Success)
{
ShowLog($"{Path.GetFileName(ImgPath)} Result:{ResMsg}", WriteLogType.Action);
}
else
{
ShowLog($"{Path.GetFileName(ImgPath)} Result:{ResMsg}", WriteLogType.Error);
}
}));
}
Task.WaitAll(Taskls.ToArray());
switch (SelectHighPass)
{
case HighPassStyle.Open:
Sufix = $"_HighPass_{50}";
RadRes = RaydiumAPI.DoCreateBin(FilePath, "./DMR_CFG.rad", Sufix);
break;
case HighPassStyle.Close:
Sufix = "";
RadRes = RaydiumAPI.DoCreateBin(FilePath, "./DMR_CFG.rad", Sufix);
break;
case HighPassStyle.ClsAndSaveOpData:
Sufix = "";
RadRes = RaydiumAPI.DoCreateBin(FilePath, "./DMR_CFG.rad", Sufix);
Sufix = $"_HighPass_{50}";
RaydiumAPI.DoCreateBin(FilePath, "./DMR_CFG.rad", Sufix);
break;
case HighPassStyle.OpAndSaveClsData:
Sufix = $"_HighPass_{50}";
RadRes = RaydiumAPI.DoCreateBin(FilePath, "./DMR_CFG.rad", Sufix);
Sufix = "";
RaydiumAPI.DoCreateBin(FilePath, "./DMR_CFG.rad", Sufix);
break;
default:
break;
}
if (RadRes != eRadErr.OK)
{
MessageBox.Show($"补偿数据生成失败！ErrorCode:{RadRes}");
return;
}
MessageBox.Show("Finished！");
}
catch (Exception ex)
{
throw new Exception(ex.Message);
}
finally
{
btn.Enabled = true;
}
}
private void CreatePicBtn_Click(object sender, EventArgs e)
{
Pattern SelectPicInfo = null;
string FilePath = FileDialog("图片|*.bmp|所有文件|*.*");
if (FilePath == "")
{
return;
}
string FileName = Path.GetFileName(FilePath);
foreach (Pattern patnInfo in SelectModelRecipe.PatternArray)
{
if (FileName.Contains(patnInfo.Color) && FileName.Contains(patnInfo.Gary.ToString()))
{
SelectPicInfo = patnInfo;
break;
}
}
string ErrorMessage = "";
StringBuilder sbPicPath = new StringBuilder(FilePath);
StringBuilder CsvPicPath = new StringBuilder(FilePath.Replace(".bmp", ".csv"));
if (this.shold1txtBox.Text == "" || this.shold2txtBox.Text == "" || this.shold3txtBox.Text == "")
{
MessageBox.Show("输入的数据不能为空！！");
return;
}
int Threshold1 = Convert.ToInt32(this.shold1txtBox.Text);
int Threshold2 = Convert.ToInt32(this.shold2txtBox.Text);
int Threshold3 = Convert.ToInt32(this.shold3txtBox.Text);
double t1 = 0, t2 = 0, t3 = 0;
float muraNum = 0;
float mThreshold1 = 0.4F;
float mThreshold2 = 0.05F;
Task.Run(() =>
{
Button btn = sender as Button;
try
{
btn.Invoke(new Action(() => { btn.Enabled = false; }));
int Res = ImgProc.ImageProc(sbPicPath, CsvPicPath, SelectPicInfo.Size, Threshold1, Threshold2,
Threshold3, SelectPicInfo.Threshold4, SelectModelRecipe.Width, SelectModelRecipe.Height,
SelectPicInfo.OffsetH, SelectPicInfo.OffsetW, SelectPicInfo.Scale, true, true, true, ref t1,
ref t2, ref t3, 100, ref muraNum, mThreshold1, mThreshold2);
ErrorMessage = ImgProc.GetErrorInfo((ImgProErrorCode)Res);
MessageBox.Show(ErrorMessage);
if (Res != (int)ImgProErrorCode.ImageError_Success)
{
ShowLog(ErrorMessage, WriteLogType.Error);
}
else
{
ShowLog(ErrorMessage, WriteLogType.Action);
}
}
catch (Exception ex)
{
throw;
}
finally
{
btn.Invoke(new Action(() => { btn.Enabled = true; }));
}
});
}
/// <summary>
/// Worker thread for image processing.
/// </summary>
public void CvMainThread()
{
var faceCascade = new CascadeClassifier();
var eyesCascade = new CascadeClassifier();
faceCascade.load("haarcascade_frontalface_alt.xml");
eyesCascade.load("haarcascade_eye_tree_eyeglasses.xml");
var srcFrame = new Mat();
var dstFrame = new Mat();
var imgProc = new ImgProc();
_videoIo.StartCapture();
while (true)
{
_videoIo.GetFrame(srcFrame);
switch (_processingMethodIndex)
{
// passthrough
case 0:
break;
// gray
case 1:
imgProc.cvtColor(srcFrame, dstFrame, ColorConversionCodes.COLOR_RGBA2GRAY);
imgProc.cvtColor(dstFrame, srcFrame, ColorConversionCodes.COLOR_GRAY2RGB);
break;
// canny
case 3:
imgProc.cvtColor(srcFrame, dstFrame, cvRT.ColorConversionCodes.COLOR_RGBA2GRAY);
imgProc.GaussianBlur(dstFrame, dstFrame, new cvRT.Size(7, 7), 1.5, 1.5);
imgProc.Canny(dstFrame, dstFrame, 0, 30, 3);
imgProc.cvtColor(dstFrame, srcFrame, ColorConversionCodes.COLOR_GRAY2RGB);
break;
// contour
case 4:
{
var contours = new VectorOfVectorOfPoint();
var hierarchy = new VectorOfVec4i();
var color = new Scalar(255, 255, 255, 255);
imgProc.Canny(srcFrame, dstFrame, 100, 100 * 2, 3);
imgProc.FindContours(dstFrame, contours, hierarchy, ContourRetrievalAlgorithm.RETR_TREE, ContourApproximationModes.CHAIN_APPROX_SIMPLE, new Point(0, 0));
srcFrame.Set(new Scalar(0, 0, 0, 0));
for (var i = 0; i < contours.Count(); i++)
{
imgProc.DrawContours(srcFrame, contours, i, color, 2, 8, hierarchy, 0, new Point(0, 0));
}
break;
}
// face detect
case 5:
{
imgProc.cvtColor(srcFrame, dstFrame, ColorConversionCodes.COLOR_RGBA2GRAY);
imgProc.EqualizeHist(dstFrame, dstFrame);
// Faces in the frame.
var faces = new List <Rect>();
try
{
faces = new List <Rect>();
faceCascade.detectMultiScale(dstFrame, faces, 1.1, 2, (int)(0 | CV_HAAR.SCALE_IMAGE), new cvRT.Size(30, 30));
}
catch (Exception ex)
{
Debug.WriteLine("Exception {0}", ex.Message);
}
// For each face, detect the eyes
foreach (var face in faces)
{
// Draw ellipse for the face.
var faceCenter = new Point(face.X + face.Width / 2, face.Y + face.Height / 2);
imgProc.Ellipse(srcFrame, faceCenter, new cvRT.Size(face.Width / 2, face.Height / 2), 0, 0, 360, new Scalar(255, 0, 255, 0), 4, 8, 0);
// Detect the eyes for the face
var faceRoi = dstFrame.RectOfInterest(face);
var eyes = new List <Rect>();
eyesCascade.detectMultiScale(faceRoi, eyes, 1.1, 2, (int)(0 | CASCADE_FLAG.CASCADE_SCALE_IMAGE), new cvRT.Size(30, 30));
// Draw the eyes
foreach (var eye in eyes)
{
var eyeCenter = new Point(face.X + eye.X + eye.Width / 2, face.Y + eye.Y + eye.Height / 2);
var radius = (int)Math.Round((eye.Width + eye.Height) * 0.25);
imgProc.Circle(srcFrame, eyeCenter, radius, new Scalar(255, 0, 0, 0), 4, 8, 0);
}
}
break;
}
default:
break;
}
_videoIo.ShowFrame(srcFrame);
}
}
public static IDisposable Create(Game game, SpriteRenderer renderer, SpriteRenderer backRenderer, PrimitiveBatchRenderer primitiveRenderer, TaskScheduler scheduler, ICommunicationManager communication)
{
var kinectStream = game.Services.GetService <IObservable <KinectFrame> >();
var markerStream = game.Services.GetService <IObservable <Tuple <IplImage, MarkerFrame> > >();
return((from magabot in Enumerable.Range(0, 1)
//let wheelClicks = 1400
let wheelClicks = 3900
let wheelDistance = 0.345f
let wheelRadius = 0.045f
let vehicle = new Vehicle()
let slam = new SlamController(vehicle)
let environment = game.Content.Load <NavigationEnvironment>("ChampalimaudLandmarks")
let slamVisualizer = new SlamVisualizer(game, backRenderer, slam, environment)
let kinectVisualizer = new KinectVisualizer(game)
let sonarVisualizer = new SonarVisualizer()
let steeringVisualizer = new SteeringVisualizer()
let freeSpaceVisualizer = new FreeSpaceVisualizer()
let actionPlayer = new ActionPlayer(game)
let markerText = new StringBuilder()
let text = new StringBuilder()
let transform = vehicle.Transform
let font = game.Content.Load <SpriteFont>("DebugFont")
let texture = game.Content.Load <Texture2D>("bot")
let bumperLeftTexture = game.Content.Load <Texture2D>("bumpLeft")
let bumperRightTexture = game.Content.Load <Texture2D>("bumpRight")
let bumperLeftOptions = new TextureDrawingParameters {
Color = Color.Transparent
}
let bumperRightOptions = new TextureDrawingParameters {
Color = Color.Transparent
}
let groundSensorLeftTexture = game.Content.Load <Texture2D>("irLeft")
let groundSensorMiddleTexture = game.Content.Load <Texture2D>("irMiddle")
let groundSensorRightTexture = game.Content.Load <Texture2D>("irRight")
let groundSensorLeftOptions = new TextureDrawingParameters {
Color = Color.White
}
let groundSensorMiddleOptions = new TextureDrawingParameters {
Color = Color.White
}
let groundSensorRightOptions = new TextureDrawingParameters {
Color = Color.White
}
let kinectTexture = new IplImageTexture(game.GraphicsDevice, 640, 480)
let bumpers = new BumperBoard(communication)
let battery = new BatteryBoard(communication)
let ground = new GroundSensorBoard(communication)
let leds = new LedBoard(communication)
let sonars = new SonarsBoard(communication)
let differentialSteering = new DifferentialSteeringBoard(communication, wheelRadius, wheelClicks)
let odometry = new OdometryBoard(communication, wheelClicks, wheelRadius, wheelDistance)
let magabotState = new MagabotState(leds, differentialSteering, bumpers, battery, ground, sonars)
let skype = new MainWindow(magabotState)
let kalman = new KalmanFilter
{
Mean = new DenseVector(3),
Covariance = new DenseMatrix(new double[, ] {
{ 1, 0, 0 },
{ 0, 1, 0 },
{ 0, 0, 1 }
})
}
//let target = new Transform2D(new Vector2(1, 0), 0, Vector2.One)
let target = new Transform2D()
let targetTexture = TextureFactory.CreateCircleTexture(game.GraphicsDevice, 2, Color.Violet)
let path = new[] { Vector2.UnitX, Vector2.One, Vector2.UnitY, Vector2.Zero, Vector2.UnitX, Vector2.One, Vector2.UnitY, Vector2.Zero, Vector2.UnitX, Vector2.One, Vector2.UnitY, Vector2.Zero, Vector2.UnitX, Vector2.One, Vector2.UnitY, Vector2.Zero, Vector2.UnitX, Vector2.One, Vector2.UnitY, Vector2.Zero, Vector2.UnitX, Vector2.One, Vector2.UnitY, Vector2.Zero, Vector2.UnitX, Vector2.One, Vector2.UnitY, Vector2.Zero, Vector2.UnitX, Vector2.One, Vector2.UnitY, Vector2.Zero, Vector2.UnitX, Vector2.One, Vector2.UnitY, Vector2.Zero, Vector2.UnitX, Vector2.One, Vector2.UnitY, Vector2.Zero }
let playActions = scheduler.TaskUpdate.Do(actionPlayer.Update)
let activateMarker = scheduler.TaskUpdate.Do(gameTime =>
{
var keyboard = Keyboard.GetState();
if (keyboard.IsKeyDown(Keys.D0))
{
ActivateMarker(actionPlayer, vehicle, environment, slam, 0, game.Services);
}
if (keyboard.IsKeyDown(Keys.D1))
{
ActivateMarker(actionPlayer, vehicle, environment, slam, 1, game.Services);
}
if (keyboard.IsKeyDown(Keys.D2))
{
ActivateMarker(actionPlayer, vehicle, environment, slam, 2, game.Services);
}
if (keyboard.IsKeyDown(Keys.D3))
{
ActivateMarker(actionPlayer, vehicle, environment, slam, 3, game.Services);
}
if (keyboard.IsKeyDown(Keys.D4))
{
ActivateMarker(actionPlayer, vehicle, environment, slam, 4, game.Services);
}
if (keyboard.IsKeyDown(Keys.D5))
{
ActivateMarker(actionPlayer, vehicle, environment, slam, 5, game.Services);
}
if (keyboard.IsKeyDown(Keys.D6))
{
ActivateMarker(actionPlayer, vehicle, environment, slam, 6, game.Services);
}
if (keyboard.IsKeyDown(Keys.D7))
{
ActivateMarker(actionPlayer, vehicle, environment, slam, 7, game.Services);
}
if (keyboard.IsKeyDown(Keys.D8))
{
ActivateMarker(actionPlayer, vehicle, environment, slam, 8, game.Services);
}
if (keyboard.IsKeyDown(Keys.D9))
{
ActivateMarker(actionPlayer, vehicle, environment, slam, 9, game.Services);
}
const string mapFileName = "map.xml";
if (keyboard.IsKeyDown(Keys.S))
{
var state = slam.StoreControllerState();
var serializer = new XmlSerializer(typeof(SlamControllerState));
using (var writer = XmlWriter.Create(mapFileName, new XmlWriterSettings {
Indent = true
}))
{
serializer.Serialize(writer, state);
}
}
if (keyboard.IsKeyDown(Keys.R) && File.Exists(mapFileName))
{
var serializer = new XmlSerializer(typeof(SlamControllerState));
using (var reader = XmlReader.Create(mapFileName))
{
var state = (SlamControllerState)serializer.Deserialize(reader);
slam.RestoreControllerState(state);
}
}
})
let obstacleCount = new Counter()
let safetyBump = magabotState.SafetyBump.MostRecent(false).GetEnumerator()
let safetyGround = magabotState.SafetyGround.MostRecent(false).GetEnumerator()
let steeringBehavior = scheduler.TaskUpdate
.Do(gameTime =>
{
var gamePad = GamePad.GetState(PlayerIndex.One);
if (gamePad.IsButtonDown(Buttons.DPadUp))
{
vehicle.Steering += (gamePad.IsButtonDown(Buttons.A) ? 1.5f : 1) * Vector2.UnitX.Rotate(vehicle.Transform.Rotation);
}
if (gamePad.IsButtonDown(Buttons.DPadDown))
{
vehicle.Steering += -Vector2.UnitX.Rotate(vehicle.Transform.Rotation);
}
if (gamePad.IsButtonDown(Buttons.DPadLeft))
{
vehicle.Steering += 0.5f * Vector2.UnitX.Rotate(vehicle.Transform.Rotation + MathHelper.PiOver2);
}
if (gamePad.IsButtonDown(Buttons.DPadRight))
{
vehicle.Steering += 0.5f * Vector2.UnitX.Rotate(vehicle.Transform.Rotation - MathHelper.PiOver2);
}
})
//.Do(Steering.PathFollow(target, path, vehicle, Steering.DefaultMinSpeed, Steering.DefaultMaxSpeed, Steering.DefaultTolerance))
.Do(gameTime =>
{
obstacleCount.Value--;
var sonarThreshold = 35;
var sonarState = magabotState.Sonar;
var rotation = 0f;
if (sonarState[0] < sonarThreshold)
{
rotation -= MathHelper.PiOver4;
}
if (sonarState[1] < sonarThreshold)
{
rotation -= MathHelper.PiOver4 * 0.5f;
}
//if (sonarState[2] < sonarThreshold) rotation += MathHelper.PiOver4;
if (sonarState[3] < sonarThreshold)
{
rotation += MathHelper.PiOver4 * 0.5f;
}
if (sonarState[4] < sonarThreshold)
{
rotation += MathHelper.PiOver4;
}
if (rotation != 0)
{
obstacleCount.Value = 90;
obstacleCount.Rotation = rotation;
}
if (obstacleCount.Value > 0)
{
vehicle.Steering = vehicle.Steering.Rotate(obstacleCount.Rotation);
}
})
.Do(gameTime => steeringVisualizer.Steering = vehicle.Steering)
//.Do(Steering.Arrival(target, vehicle, 1, 3, 0.3f))
.Where(gameTime => magabotState.Stopped)
.Where(gameTime => { safetyBump.MoveNext(); return !safetyBump.Current; })
//.Where(gameTime => { safetyGround.MoveNext(); return !safetyGround.Current; })
.Do(Locomotion.DifferentialSteering(vehicle, differentialSteering, wheelDistance, MathHelper.Pi / 16, 10, 100, 3))
let visualizerLoop = scheduler.TaskUpdate
.Do(time => slamVisualizer.Update())
.Do(time => kinectTexture.Update())
let behavior = scheduler.TaskUpdate
.Do(time => odometry.UpdateOdometryCommand())
.Do(time => magabotState.DifferentialSteering.UpdateWheelVelocity(new WheelVelocity(0, 0)))
.Do(time => magabotState.Leds.SetLedBoardState(255, 255, 255))
.Do(time => skype.Magabot = magabotState)
.Do(time => skype.Show())
.Take(1)
select new CompositeDisposable(
bumpers,
battery,
ground,
leds,
sonars,
odometry,
differentialSteering,
playActions.Subscribe(),
activateMarker.Subscribe(),
steeringBehavior.Subscribe(),
visualizerLoop.Subscribe(),
//backRenderer.SubscribeTexture(new Transform2D(new Vector2(-2.75f, 1.7f), 0, new Vector2(0.25f)), kinectTexture.Texture),
backRenderer.SubscribeTexture(new Transform2D(new Vector2(-4.25f, 2.0f), 0, new Vector2(0.75f)), kinectTexture.Texture),
renderer.SubscribeTexture(transform, texture),
renderer.SubscribeTexture(transform, new Transform2D(), bumperLeftTexture, bumperLeftOptions),
renderer.SubscribeTexture(transform, new Transform2D(), bumperRightTexture, bumperRightOptions),
renderer.SubscribeTexture(transform, new Transform2D(), groundSensorLeftTexture, groundSensorLeftOptions),
renderer.SubscribeTexture(transform, new Transform2D(), groundSensorMiddleTexture, groundSensorMiddleOptions),
renderer.SubscribeTexture(transform, new Transform2D(), groundSensorRightTexture, groundSensorRightOptions),
renderer.SubscribeTexture(target, targetTexture),
//renderer.SubscribeText(transform, font, () => text.ToString()),
renderer.SubscribeText(new Transform2D(-Vector2.One, 0, Vector2.One), font, () => markerText.ToString(), Color.White),
primitiveRenderer.SubscribePrimitive(transform, kinectVisualizer.DrawKinectDepthMap),
primitiveRenderer.SubscribePrimitive(transform, sonarVisualizer.DrawSonarFrame),
primitiveRenderer.SubscribePrimitive(transform, steeringVisualizer.DrawSteeringVector),
primitiveRenderer.SubscribePrimitive(new Transform2D(new Vector2(-350, 200), 0, Vector2.One), freeSpaceVisualizer.DrawFreeSpace),
behavior.Subscribe(),
magabotState.MarkerActivated.Subscribe(name => ActivateMarker(actionPlayer, vehicle, environment, slam, name, game.Services)),
kinectStream.Subscribe(kinectFrame =>
{
kinectVisualizer.Frame = kinectFrame;
freeSpaceVisualizer.FreeSpace = KinectFreeSpace.ComputeFreeSpace(kinectFrame, 1500);
skype.OnKinectFrame(kinectFrame);
}),
markerStream.Subscribe(markerOutput =>
{
var inputImage = markerOutput.Item1;
var image = new IplImage(new CvSize(kinectTexture.Texture.Width, kinectTexture.Texture.Height), inputImage.Depth, inputImage.NumChannels);
if (inputImage.Width != kinectTexture.Texture.Width || inputImage.Height != kinectTexture.Texture.Height)
{
ImgProc.cvResize(inputImage, image, SubPixelInterpolation.NearestNeighbor);
}
else
{
Core.cvCopy(inputImage, image);
}
foreach (var marker in markerOutput.Item2.DetectedMarkers)
{
marker.Draw(image.DangerousGetHandle(), 0, 0, 255, 2, true);
}
kinectTexture.SetData(image);
slam.UpdateMeasurements(markerOutput.Item2);
}),
differentialSteering.CommandChecksum.Subscribe(m => bumpers.GetBumperState()),
bumpers.BumpersMeasure.Subscribe(m =>
{
bumperLeftOptions.Color = m.BumperLeft ? Color.White : Color.Transparent;
bumperRightOptions.Color = m.BumperRight ? Color.White : Color.Transparent;
battery.GetBatteryState();
}),
battery.BatteryMeasure.Subscribe(m =>
{
text.Clear();
text.AppendLine(string.Format("Battery: {0}", m.ToString()));
ground.GetGroundSensorState();
}),
ground.GroundSensorsMeasure.Subscribe(m =>
{
var sensorScale = 180 / 1023f;
groundSensorLeftOptions.Color = GraphicsHelper.HsvToRgb(new Vector3(180 - m.SensorLeft * sensorScale, 1, 1));
groundSensorMiddleOptions.Color = GraphicsHelper.HsvToRgb(new Vector3(180 - m.SensorMiddle * sensorScale, 1, 1));
groundSensorRightOptions.Color = GraphicsHelper.HsvToRgb(new Vector3(180 - m.SensorRight * sensorScale, 1, 1));
text.AppendLine(string.Format("IR: {0} IR: {1} IR: {2}", m.SensorLeft, m.SensorMiddle, m.SensorRight));
sonars.GetSonarsBoardState();
}),
sonars.SonarsBoardMeasure.Subscribe(m =>
{
sonarVisualizer.SonarFrame = m;
for (int count = 0; count < m.Length; count++)
{
var sonar = m[count];
text.Append(string.Format("Sonar: {0} ", sonar));
}
text.AppendLine();
leds.Actuate();
}),
leds.LedBoardMeasure.Subscribe(m => odometry.UpdateOdometryCommand()),
odometry.Odometry.Subscribe(m =>
{
slam.UpdateMotion(m.LinearDisplacement, m.AngularDisplacement);
slam.UpdateEstimate();
differentialSteering.Actuate();
})))
.First());
}
