ASUS Xtion pre robota SmelyZajko (Marek Jelen, Miroslav Garai)

Zadanie

Našou úlohou bolo pripojiť na robota SmelyZajko zariadenie Asus Xtion pro, pomocou neho detekovať prekážky a bezpečne ich obísť.

Asus Xtion Pro

Špecifikácia:

- Používateľná vzďialenosť: mezi 0.8m a 3.5m

- Zobrazovacie pole: 58° H, 45° V, 70° D (Horizontálne, Vertikálne, Diagonálne)

- Sensor: hĺbkový

- Veľkosť obrázku: VGA (640x480) : 30fps

Inštalácia: Potrebné na rozbehanie ASUS Xtion PRO bol Prime sense. Je to open soure ovládač pre snímač RGB-D. Ďalší potrebný driver je OpenNi. Sú tam tiež príklady zdrojvých kódov pre prácu s ASUS Xtion PRO. Spojazdnili sme ho na operačnom systéme Windows 7. Na školskom notebooku, ktorý je súčasť robota je operačný systém Linux. Asus Xtion sme spojazdnili aj tam. No mali sme menšie komplikácie. Dodávaný OpenNI na inštalačnom CD nefungoval a museli sme stiahnuť z GitHubu novšiu verziu s kotoru to už potom fungovalo. Chceli sme to však programovať v jazyku C#, ktorý na Linux nepodporuje tak sme použili vlastný notebook s Windowsom.

Programovanie robota

Na ovládanie robota sme použili vlastný notebook, ktorý s robotom komunikoval cez sériový port. Na vývoj aplikácie sme použili Miscrosoft Visual Studio a programovali sme v jazyku C#. Na začiatku programu sa spustia 3 thredy, ktoré riadia celý proces obchádzania.

• ReaderThread - komunikuje so zariadením Asus Xtion, na počítači z neho zobrazuje pohľad a detekuje prekážky. Až nejakú nájde, povie Thredu na obchádzanie, či ju má obísť zprava alebo zľava
• ObchadzajThread - čaká na pokyn od ReaderThreadu, či nemá urobiť vyhýbací manéver. Keď dostane pokyn, začne CitajThreadu odosielať príkazy pre robota.
• CitajThread - neustále číta vstupy, ktoré robot posiela a kontorluje, či mu ObidThread neposlal njeaké príkazy pre robota, ak áno tak ich cez sériový port odošle

private void CitajThread()
{
    while (true)
    {
         while (_serialPort.BytesToRead > 0)
              Console.Write(_serialPort.ReadChar());
         if (this.zapis != false)
         {
              Console.WriteLine("pisem");
              _serialPort.WriteLine(this.posli);
              this.zapis = false;
         }
    }            
}

private unsafe void ReaderThread()
{
     DepthMetaData depthMD = new DepthMetaData();
     while (this.shouldRun)
     {
          try
          {
               this.context.WaitOneUpdateAll(this.depth);
          }
          catch (Exception)
          {
          }           
          this.depth.GetMetaData(depthMD);
          CalcHist(depthMD);
          lock (this)
          {
               Rectangle rect = new Rectangle(0, 0, this.bitmap.Width, this.bitmap.Height);
               BitmapData data = this.bitmap.LockBits(rect, ImageLockMode.WriteOnly, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
               ushort* pDepth = (ushort*)this.depth.DepthMapPtr.ToPointer();
               // set pixels
               for (int y = 0; y < depthMD.YRes; ++y)
               {
                    byte* pDest = (byte*)data.Scan0.ToPointer() + y * data.Stride;
                    for (int x = 0; x < depthMD.XRes; ++x, ++pDepth, pDest += 3)
                    {
                         byte pixel = (byte)this.histogram[*pDepth];
                         if ((pixel > 250) && (y>30) && (y<200))
                         {
                              pDest[0] = 0;
                              pDest[1] = 0;
                              pDest[2] = 0;
                              if ((x > 100) && (check != true))
                              {
                                   this.pocitadlo += 1;
                                   if (this.pocitadlo > 5)
                                   {
                                       Console.WriteLine("zprava");
                                       this.vlavo = false;
                                       this.obchadzaj = true;
                                       this.pocitadlo = 0;
                                   }                               
                               }
                               else if (check != true)
                               {
                                   this.pocitadlo += 1;
                                   if (this.pocitadlo > 5)
                                   {
                                       Console.WriteLine("zlava");
                                       this.vlavo = true;
                                       this.obchadzaj = true;
                                       this.pocitadlo = 0;
                                   }
                                   
                               }
                          }
                          else
                          {
                               pDest[0] = 255;
                               pDest[1] = 255;
                               pDest[2] = pixel;
                          }
                    }
               }
          this.bitmap.UnlockBits(data);
          }
          this.Invalidate();
     }
}

Zdrojové súbory

Projekt