NXT gesture control with Kinect

Levin Eersink asked:

Hello,
my name is Levin I'm from germany and I go to a technical school.
I have made a project with the Kinect Sensor and the NXT Mindstorm, using gesture I steered the NXT.
But now I have a problem.
If the Kinect recognizes my skeleton it will not work, but when I hold my right hand over my Head the NXT drive.
Sorry if I have errors in spelling :(

Thanks in advance!

Now here my C# Code:
using System;
using System.IO;
using System.Linq;
using System.Text;
using System.Windows;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using Microsoft.Kinect;
using AForge;
using AForge.Robotics.Lego;


namespace KinectDemo1
{

    public partial class MainWindow : Window
    {


        public MainWindow()
        {
            InitializeComponent();
        }

        
                           
                    
        public KinectSensor sensor;
    
        public NXTBrick nxt = new NXTBrick();



        private void Window_Loaded(object sender, RoutedEventArgs e)
        {          
            this.drawingGroup = new DrawingGroup();//Skeleton


            this.imageSource = new DrawingImage(this.drawingGroup);//Skeleton


            image3.Source = this.imageSource;//Skeleton
                                
                    
            foreach (var potentialSensor in KinectSensor.KinectSensors)//Skeleton
            {
                if (potentialSensor.Status == KinectStatus.Connected)
                {
                    sensor = potentialSensor;
                    break;
                }
            }
                                

                    

            sensor = (from Sensor in KinectSensor.KinectSensors where Sensor.Status == KinectStatus.Connected select Sensor).FirstOrDefault();




            if (sensor != null)
            {

                label9.Content = "Kinect ist verbunden!";
                    
                sensor.ColorFrameReady += new EventHandler<ColorImageFrameReadyEventArgs>(r_ColorFrameReady);//RGB-Bild

                sensor.ColorStream.Enable(ColorImageFormat.RgbResolution640x480Fps30);//RGB-Bild

                sensor.SkeletonStream.Enable();//Skeleton

                sensor.SkeletonFrameReady += new EventHandler<SkeletonFrameReadyEventArgs>(SensorSkeletonFrameReady);

                sensor.SkeletonFrameReady += this.SensorSkeletonFrameReady;//Skeleton

                sensor.Start();


            }
            else
            {
                label9.Content = "Kinect nicht verbunden!";
                MessageBox.Show("Keine Kinect angeschlossen.", "Fehler", MessageBoxButton.OK, MessageBoxImage.Hand);
            }



            //Akkustand des Mindstorm Roboter
            nxt.Connect("COM3");

            int Akkustand;

            if (nxt.GetBatteryPower(out Akkustand))
            {
                label6.Content = Akkustand.ToString();
                progressBar1.Value = Akkustand;
            }

            //NXT verbunden?
            if (nxt.Connect("COM3"))
            {
                label8.Content = "Mindstorm ist verbunden!";
            }

        }

        private void Window_Closed(object sender, EventArgs e)
        {
            if (sensor != null)
            {
                sensor.Stop();
                sensor.Dispose();
            }
        }





        #region RGB-Bild der Kinect

        void r_ColorFrameReady(object sender, ColorImageFrameReadyEventArgs e)// Ausgabe des RGB Bildes
        {
            using (ColorImageFrame frame = e.OpenColorImageFrame())// Werte von der Kamera werden ausgegeben
            {
                if (frame == null)
                    return;

                byte[] buffer = new byte[frame.PixelDataLength];// Pixeldatengröße wird errstellt
                frame.CopyPixelDataTo(buffer);
                image1.Source = BitmapSource.Create(frame.Width, frame.Height, 96, 96, PixelFormats.Bgr32, null, buffer, frame.Width * frame.BytesPerPixel);
            }
        }

        #endregion



        #region SkeletonView


        private const float RenderWidth = 800.0f; //Größenfaktor (je größer die Zahl desto kleiner das Skeleton)

        private const float RenderHeight = 600.0f;//Größenfaktor (je größer die Zahl desto kleiner das Skeleton)

        private const double JointThickness = 3;//Größenfaktor (je größer die Zahl desto größer die Gelenkzeichnungen)

        private const double BodyCenterThickness = 10;

        private const double ClipBoundsThickness = 10;

        private readonly Brush centerPointBrush = Brushes.Blue;

        private readonly Brush trackedJointBrush = new SolidColorBrush(Color.FromArgb(255, 68, 192, 68));

        private readonly Brush inferredJointBrush = Brushes.Yellow;

        private readonly Pen trackedBonePen = new Pen(Brushes.Green, 6);

        private readonly Pen inferredBonePen = new Pen(Brushes.Gray, 1); 

        private DrawingGroup drawingGroup;

        private DrawingImage imageSource;


        private void SensorSkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
        {
            Skeleton[] skeletons = new Skeleton[0];

            using (SkeletonFrame skeletonFrame = e.OpenSkeletonFrame())
            {
                if (skeletonFrame != null)
                {
                    skeletons = new Skeleton[skeletonFrame.SkeletonArrayLength];
                    skeletonFrame.CopySkeletonDataTo(skeletons);
                }
            }

            using (DrawingContext dc = this.drawingGroup.Open())
            {

                dc.DrawRectangle(Brushes.Black, null, new Rect(0.0, 0.0, RenderWidth, RenderHeight));

                if (skeletons.Length != 0)
                {
                    foreach (Skeleton skel in skeletons)
                    {


                        if (skel.TrackingState == SkeletonTrackingState.Tracked)
                        {
                            this.DrawBonesAndJoints(skel, dc);
                            this.nxtDrive(skel);
                        }
                        else if (skel.TrackingState == SkeletonTrackingState.PositionOnly)
                        {
                            dc.DrawEllipse(
                            this.centerPointBrush,
                            null,
                            this.SkeletonPointToScreen(skel.Position),
                            BodyCenterThickness,
                            BodyCenterThickness);
                        }
                    }
                }


                this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, RenderWidth, RenderHeight));
            }
        }


        private void DrawBonesAndJoints(Skeleton skeleton, DrawingContext drawingContext)
        {

            this.DrawBone(skeleton, drawingContext, JointType.Head, JointType.ShoulderCenter);
            this.DrawBone(skeleton, drawingContext, JointType.ShoulderCenter, JointType.ShoulderLeft);
            this.DrawBone(skeleton, drawingContext, JointType.ShoulderCenter, JointType.ShoulderRight);
            this.DrawBone(skeleton, drawingContext, JointType.ShoulderCenter, JointType.Spine);
            this.DrawBone(skeleton, drawingContext, JointType.Spine, JointType.HipCenter);
            this.DrawBone(skeleton, drawingContext, JointType.HipCenter, JointType.HipLeft);
            this.DrawBone(skeleton, drawingContext, JointType.HipCenter, JointType.HipRight);


            this.DrawBone(skeleton, drawingContext, JointType.ShoulderLeft, JointType.ElbowLeft);
            this.DrawBone(skeleton, drawingContext, JointType.ElbowLeft, JointType.WristLeft);
            this.DrawBone(skeleton, drawingContext, JointType.WristLeft, JointType.HandLeft);


            this.DrawBone(skeleton, drawingContext, JointType.ShoulderRight, JointType.ElbowRight);
            this.DrawBone(skeleton, drawingContext, JointType.ElbowRight, JointType.WristRight);
            this.DrawBone(skeleton, drawingContext, JointType.WristRight, JointType.HandRight);


            this.DrawBone(skeleton, drawingContext, JointType.HipLeft, JointType.KneeLeft);
            this.DrawBone(skeleton, drawingContext, JointType.KneeLeft, JointType.AnkleLeft);
            this.DrawBone(skeleton, drawingContext, JointType.AnkleLeft, JointType.FootLeft);


            this.DrawBone(skeleton, drawingContext, JointType.HipRight, JointType.KneeRight);
            this.DrawBone(skeleton, drawingContext, JointType.KneeRight, JointType.AnkleRight);
            this.DrawBone(skeleton, drawingContext, JointType.AnkleRight, JointType.FootRight);


            foreach (Joint joint in skeleton.Joints)
            {
                Brush drawBrush = null;

                if (joint.TrackingState == JointTrackingState.Tracked)
                {
                    drawBrush = this.trackedJointBrush;
                }
                else if (joint.TrackingState == JointTrackingState.Inferred)
                {
                    drawBrush = this.inferredJointBrush;
                }

                if (drawBrush != null)
                {
                    drawingContext.DrawEllipse(drawBrush, null, this.SkeletonPointToScreen(joint.Position), JointThickness, JointThickness);
                }
            }







        }


        private Point SkeletonPointToScreen(SkeletonPoint skelpoint)
        {

            DepthImagePoint depthPoint = this.sensor.CoordinateMapper.MapSkeletonPointToDepthPoint(skelpoint, DepthImageFormat.Resolution640x480Fps30);
            return new Point(depthPoint.X, depthPoint.Y);
        }


        private void DrawBone(Skeleton skeleton, DrawingContext drawingContext, JointType jointType0, JointType jointType1)
        {
            Joint joint0 = skeleton.Joints[jointType0];
            Joint joint1 = skeleton.Joints[jointType1];


            if (joint0.TrackingState == JointTrackingState.NotTracked ||
                joint1.TrackingState == JointTrackingState.NotTracked)
            {
                return;
            }


            if (joint0.TrackingState == JointTrackingState.Inferred &&
                joint1.TrackingState == JointTrackingState.Inferred)
            {
                return;
            }


            Pen drawPen = this.inferredBonePen;
            if (joint0.TrackingState == JointTrackingState.Tracked && joint1.TrackingState == JointTrackingState.Tracked)
            {
                drawPen = this.trackedBonePen;
            }

            drawingContext.DrawLine(drawPen, this.SkeletonPointToScreen(joint0.Position), this.SkeletonPointToScreen(joint1.Position));
        }

        #endregion



        #region Manuelle fahrt des Mindstorm Roboter



        // Vorwärts
        private void button1_MouseEnter(object sender, System.Windows.Input.MouseEventArgs e)
        {
            nxt.Connect("COM3");

            NXTBrick.MotorState motorState = new NXTBrick.MotorState();

            motorState.Power = 100;
            motorState.TurnRatio = 50;  
            motorState.Mode = NXTBrick.MotorMode.On;
            motorState.Regulation = NXTBrick.MotorRegulationMode.Idle;
            motorState.RunState = NXTBrick.MotorRunState.Running;
            motorState.TachoLimit = 0;

            
            nxt.SetMotorState(NXTBrick.Motor.B, motorState);
           
            nxt.SetMotorState(NXTBrick.Motor.C, motorState);
        }

        private void button1_MouseLeave(object sender, System.Windows.Input.MouseEventArgs e)
        {
            nxt.Connect("COM3");

            NXTBrick.MotorState motorState = new NXTBrick.MotorState();

            motorState.Power = 0;

            nxt.SetMotorState(NXTBrick.Motor.All, motorState);

        }



        // Rückwärts
        private void button2_MouseEnter(object sender, System.Windows.Input.MouseEventArgs e)
        {
            nxt.Connect("COM3");

            NXTBrick.MotorState motorState = new NXTBrick.MotorState();

            motorState.Power = -100;
            motorState.TurnRatio = 50;
            motorState.Mode = NXTBrick.MotorMode.On;
            motorState.Regulation = NXTBrick.MotorRegulationMode.Idle;
            motorState.RunState = NXTBrick.MotorRunState.Running;
            motorState.TachoLimit = 0;


            nxt.SetMotorState(NXTBrick.Motor.B, motorState);

            nxt.SetMotorState(NXTBrick.Motor.C, motorState);
        }

        private void button2_MouseLeave(object sender, System.Windows.Input.MouseEventArgs e)
        {
            nxt.Connect("COM3");

            NXTBrick.MotorState motorState = new NXTBrick.MotorState();

            motorState.Power = 0;

            nxt.SetMotorState(NXTBrick.Motor.All, motorState);
        }



        // Links
        private void button3_MouseEnter(object sender, System.Windows.Input.MouseEventArgs e)
        {
            nxt.Connect("COM3");

            NXTBrick.MotorState motorState = new NXTBrick.MotorState();

            
            motorState.TurnRatio = 50;
            motorState.Mode = NXTBrick.MotorMode.On;
            motorState.Regulation = NXTBrick.MotorRegulationMode.Idle;
            motorState.RunState = NXTBrick.MotorRunState.Running;
            motorState.TachoLimit = 0;

            motorState.Power = 100;
            nxt.SetMotorState(NXTBrick.Motor.B, motorState);
            motorState.Power = -100;
            nxt.SetMotorState(NXTBrick.Motor.C, motorState);
        }

        private void button3_MouseLeave(object sender, System.Windows.Input.MouseEventArgs e)
        {
            nxt.Connect("COM3");

            NXTBrick.MotorState motorState = new NXTBrick.MotorState();

            motorState.Power = 0;

            nxt.SetMotorState(NXTBrick.Motor.All, motorState);
        }



        // Rechts
        private void button4_MouseEnter(object sender, System.Windows.Input.MouseEventArgs e)
        {
            nxt.Connect("COM3");

            NXTBrick.MotorState motorState = new NXTBrick.MotorState();


            motorState.TurnRatio = 50;
            motorState.Mode = NXTBrick.MotorMode.On;
            motorState.Regulation = NXTBrick.MotorRegulationMode.Idle;
            motorState.RunState = NXTBrick.MotorRunState.Running;
            motorState.TachoLimit = 0;

            motorState.Power = -100;
            nxt.SetMotorState(NXTBrick.Motor.B, motorState);
            motorState.Power = 100;
            nxt.SetMotorState(NXTBrick.Motor.C, motorState);
        }

        private void button4_MouseLeave(object sender, System.Windows.Input.MouseEventArgs e)
        {
            nxt.Connect("COM3");

            NXTBrick.MotorState motorState = new NXTBrick.MotorState();

            motorState.Power = 0;

            nxt.SetMotorState(NXTBrick.Motor.All, motorState);
        }



        // Schießen
        private void button5_MouseEnter(object sender, System.Windows.Input.MouseEventArgs e)
        {
            nxt.Connect("COM3");

            NXTBrick.MotorState motorState = new NXTBrick.MotorState();

            motorState.TurnRatio = 50;
            motorState.Mode = NXTBrick.MotorMode.On;
            motorState.Regulation = NXTBrick.MotorRegulationMode.Idle;
            motorState.RunState = NXTBrick.MotorRunState.Running;
            motorState.TachoLimit = 360;

            motorState.Power = 90;
            nxt.SetMotorState(NXTBrick.Motor.A, motorState);

        }

        private void button5_MouseLeave(object sender, System.Windows.Input.MouseEventArgs e)
        {
            nxt.Connect("COM3");

            NXTBrick.MotorState motorState = new NXTBrick.MotorState();

            motorState.Power = 0;

            nxt.SetMotorState(NXTBrick.Motor.All, motorState);
        }


        #endregion



        #region nxtDrive


        private void nxtDrive(Skeleton skeleton)
        {
            SkeletonPoint headPos = skeleton.Joints[JointType.Head].Position;
            SkeletonPoint righthandPos = skeleton.Joints[JointType.HandRight].Position;
            SkeletonPoint lefthandPos = skeleton.Joints[JointType.HandLeft].Position;

            bool LinkeHandÜberKopf = (headPos.Y < lefthandPos.Y);

            bool RechteHandÜberKopf = (headPos.Y < righthandPos.Y);

            if (nxt.Connect("COM3"))
            {

                if (RechteHandÜberKopf && LinkeHandÜberKopf)
                {




                    NXTBrick.MotorState motorState = new NXTBrick.MotorState();

                    motorState.Power = 100;
                    motorState.TurnRatio = 50;
                    motorState.Mode = NXTBrick.MotorMode.On;
                    motorState.Regulation = NXTBrick.MotorRegulationMode.Idle;
                    motorState.RunState = NXTBrick.MotorRunState.Running;
                    motorState.TachoLimit = 0;


                    nxt.SetMotorState(NXTBrick.Motor.B, motorState);

                    nxt.SetMotorState(NXTBrick.Motor.C, motorState);

                }

                else
                {


                    NXTBrick.MotorState motorState = new NXTBrick.MotorState();

                    motorState.Power = 0;

                    nxt.SetMotorState(NXTBrick.Motor.All, motorState);
                }



                if (RechteHandÜberKopf)
                {




                    NXTBrick.MotorState motorState = new NXTBrick.MotorState();

                    motorState.Power = 100;
                    motorState.TurnRatio = 50;
                    motorState.Mode = NXTBrick.MotorMode.On;
                    motorState.Regulation = NXTBrick.MotorRegulationMode.Idle;
                    motorState.RunState = NXTBrick.MotorRunState.Running;
                    motorState.TachoLimit = 0;



                    nxt.SetMotorState(NXTBrick.Motor.C, motorState);




                }

                else
                {


                    NXTBrick.MotorState motorState = new NXTBrick.MotorState();

                    motorState.Power = 0;

                    nxt.SetMotorState(NXTBrick.Motor.All, motorState);


                }

                if (LinkeHandÜberKopf)
                {




                    NXTBrick.MotorState motorState = new NXTBrick.MotorState();

                    motorState.Power = 100;
                    motorState.TurnRatio = 50;
                    motorState.Mode = NXTBrick.MotorMode.On;
                    motorState.Regulation = NXTBrick.MotorRegulationMode.Idle;
                    motorState.RunState = NXTBrick.MotorRunState.Running;
                    motorState.TachoLimit = 0;


                    nxt.SetMotorState(NXTBrick.Motor.B, motorState);
                }

                else
                {


                    NXTBrick.MotorState motorState = new NXTBrick.MotorState();

                    motorState.Power = 0;

                    nxt.SetMotorState(NXTBrick.Motor.All, motorState);
                }
            }

            else
            {
                MessageBox.Show("Es besteht keine Bluetooth verbindung zum Mindstorm!", "Fehler", MessageBoxButton.OK, MessageBoxImage.Warning);
            }

        }


        #endregion


    }

 }
Tags: C#, Kinect, NXT
Treat my content as plain text, not as HTML
Preview 0

…
Existing Members

...or Join us

Download, Vote, Comment, Publish.
Your Email
Password
Forgot your password?
Your Email
This email is in use. Do you need your password?
Optional Password
I have read and agree to the Terms of Service and Privacy Policy
Please subscribe me to the CodeProject newsletters
Cancel
When answering a question please:
Read the question carefully.
Understand that English isn't everyone's first language so be lenient of bad spelling and grammar.
If a question is poorly phrased then either ask for clarification, ignore it, or edit the question and fix the problem. Insults are not welcome.
Don't tell someone to read the manual. Chances are they have and don't get it. Provide an answer or move on to the next question.
Let's work to help developers, not make them feel stupid.
Please note that all posts will be submitted under the http://www.codeproject.com/info/cpol10.aspx.