Esempio n. 1
0
        public override void Randomize()
        {
            int    arraylength = -1;
            Random rand        = new Random();
            int    strlength;

            byte[] strbuf, myByte;

            //twist
            twist = new Twist();
            twist.Randomize();
            //covariance
            if (covariance == null)
            {
                covariance = new double[36];
            }
            else
            {
                Array.Resize(ref covariance, 36);
            }
            for (int i = 0; i < covariance.Length; i++)
            {
                //covariance[i]
                covariance[i] = (rand.Next() + rand.NextDouble());
            }
        }
Esempio n. 2
0
        public override byte[] Serialize(bool partofsomethingelse)
        {
            int  currentIndex = 0, length = 0;
            bool hasmetacomponents = false;

            byte[]        thischunk, scratch1, scratch2;
            List <byte[]> pieces = new List <byte[]>();
            GCHandle      h;

            //header
            if (header == null)
            {
                header = new Header();
            }
            pieces.Add(header.Serialize(true));
            //twist
            if (twist == null)
            {
                twist = new Twist();
            }
            pieces.Add(twist.Serialize(true));
            //combine every array in pieces into one array and return it
            int __a_b__f = pieces.Sum((__a_b__c) => __a_b__c.Length);
            int __a_b__e = 0;

            byte[] __a_b__d = new byte[__a_b__f];
            foreach (var __p__ in pieces)
            {
                Array.Copy(__p__, 0, __a_b__d, __a_b__e, __p__.Length);
                __a_b__e += __p__.Length;
            }
            return(__a_b__d);
        }
Esempio n. 3
0
        public void pubCmdMsg(double linear, double angular, double speed)
        {
            Messages.geometry_msgs.Twist msg;
            msg          = new Messages.geometry_msgs.Twist();
            msg.linear   = new Messages.geometry_msgs.Vector3();
            msg.linear.x = linear * speed > 0?(block_up?0:linear * speed):(block_down?0:linear * speed);

            msg.angular   = new Messages.geometry_msgs.Vector3();
            msg.angular.z = angular * speed;
            pub.publish(msg);
        }
Esempio n. 4
0
        public override void Deserialize(byte[] SERIALIZEDSTUFF, ref int currentIndex)
        {
            int    arraylength       = -1;
            bool   hasmetacomponents = false;
            object __thing;
            int    piecesize = 0;

            byte[] thischunk, scratch1, scratch2;
            IntPtr h;

            //header
            header = new Header(SERIALIZEDSTUFF, ref currentIndex);
            //twist
            twist = new Twist(SERIALIZEDSTUFF, ref currentIndex);
        }
Esempio n. 5
0
        public override void Randomize()
        {
            int    arraylength = -1;
            Random rand        = new Random();
            int    strlength;

            byte[] strbuf, myByte;

            //header
            header = new Header();
            header.Randomize();
            //twist
            twist = new Twist();
            twist.Randomize();
        }
Esempio n. 6
0
        public override byte[] Serialize(bool partofsomethingelse)
        {
            int  currentIndex = 0, length = 0;
            bool hasmetacomponents = false;

            byte[]        thischunk, scratch1, scratch2;
            List <byte[]> pieces = new List <byte[]>();
            GCHandle      h;

            //twist
            if (twist == null)
            {
                twist = new Twist();
            }
            pieces.Add(twist.Serialize(true));
            //covariance
            hasmetacomponents |= false;
            if (covariance == null)
            {
                covariance = new double[0];
            }
            for (int i = 0; i < covariance.Length; i++)
            {
                //covariance[i]
                scratch1 = new byte[Marshal.SizeOf(typeof(double))];
                h        = GCHandle.Alloc(scratch1, GCHandleType.Pinned);
                Marshal.StructureToPtr(covariance[i], h.AddrOfPinnedObject(), false);
                h.Free();
                pieces.Add(scratch1);
            }
            //combine every array in pieces into one array and return it
            int __a_b__f = pieces.Sum((__a_b__c) => __a_b__c.Length);
            int __a_b__e = 0;

            byte[] __a_b__d = new byte[__a_b__f];
            foreach (var __p__ in pieces)
            {
                Array.Copy(__p__, 0, __a_b__d, __a_b__e, __p__.Length);
                __a_b__e += __p__.Length;
            }
            return(__a_b__d);
        }
    void TwistCallback(Twist msg)
    {
        if (inputCtrl.localInput)
        {
            return;
        }

        Vector3 linear  = msg.linear.ToUnityVector();
        Vector3 angular = msg.angular.ToUnityVector();

        if (droneController != null)
        {
            if (!droneController.MotorsEnabled)
            {
                droneController.MotorsEnabled = true;
            }
            droneController.SetLinearVelocity(linear, true);
            droneController.SetAngularVelocity(angular, true);
        }
    }
Esempio n. 8
0
        public override void Deserialize(byte[] SERIALIZEDSTUFF, ref int currentIndex)
        {
            int    arraylength       = -1;
            bool   hasmetacomponents = false;
            object __thing;
            int    piecesize = 0;

            byte[] thischunk, scratch1, scratch2;
            IntPtr h;

            //twist
            twist = new Twist(SERIALIZEDSTUFF, ref currentIndex);
            //covariance
            hasmetacomponents |= false;
            if (covariance == null)
            {
                covariance = new double[36];
            }
            else
            {
                Array.Resize(ref covariance, 36);
            }
            for (int i = 0; i < covariance.Length; i++)
            {
                //covariance[i]
                piecesize = Marshal.SizeOf(typeof(double));
                h         = IntPtr.Zero;
                if (SERIALIZEDSTUFF.Length - currentIndex != 0)
                {
                    h = Marshal.AllocHGlobal(piecesize);
                    Marshal.Copy(SERIALIZEDSTUFF, currentIndex, h, piecesize);
                }
                if (h == IntPtr.Zero)
                {
                    throw new Exception("Alloc failed");
                }
                covariance[i] = (double)Marshal.PtrToStructure(h, typeof(double));
                Marshal.FreeHGlobal(h);
                currentIndex += piecesize;
            }
        }
Esempio n. 9
0
        // controller link dispatcher
        public void Link(object sender, EventArgs dontcare)
        {
            // get state of player one
            currentState = GamePad.GetState(PlayerIndex.One);

            // if controller is connected...
            if (currentState.IsConnected)
            {
                // ...say its connected in the textbloxk, color it green cuz its good to go
                LinkTextBlock.Text = "Controller: Connected";
                LinkTextBlock.Foreground = Brushes.Green;

                foreach (Buttons b in Enum.GetValues(typeof(Buttons)))
                {
                    Button(b);
                }
                double left_y = currentState.ThumbSticks.Left.Y;
                double left_x = -currentState.ThumbSticks.Left.X;            
                
                if (_adr)
                {
                    //left_x *= -1;
                    left_y *= -1;
                }
                gm.Twist vel = new gm.Twist { linear = new gm.Vector3 { x = left_y * _trans.Value }, angular = new gm.Vector3 { z = left_x * _rot.Value } };
                if(velPub != null)
                    velPub.publish(vel);
                
                //arm controls via joystick are done here.
                double right_y = currentState.ThumbSticks.Right.Y;

                // this is inverted to reflect mikes arm driver.  right requires a negative number, not the default positive value
                double right_x = -1 * currentState.ThumbSticks.Right.X; 
                double right_trigger = currentState.Triggers.Right;

                if (!engaged && (Math.Abs(right_y) > .1 || Math.Abs(right_x) > .1 ))
                {
                    engaged = true;
                    ArmON.publish(new m.Bool() { data = true });
                    Arm_Engaged.Content = "Arm Engaged";
                    Arm_Engaged.Background = Brushes.White;
                    Arm_Engaged.Foreground = Brushes.Green;
                }

                //if trigger is not pressed, send close signal ( -1 ).  Th goal is to have the gripper
                // going to a close state when the right trigger is not being pressed.
                if (right_trigger == 0)
                    right_trigger = -1;

                /*Console.WriteLine( "joy_right_x: " + right_x.ToString());
                Console.WriteLine( "joy_right_y: " + right_y.ToString());
                Console.WriteLine( "right trigger: " + right_trigger.ToString());*/

                am.ArmMovement armmove = new am.ArmMovement();

                armmove.pan_motor_velocity = right_x;
                armmove.tilt_motor_velocity = right_y;
                armmove.gripper_open = (right_trigger >= 0.5);

                if (armPub != null)
                    armPub.publish(armmove);

            }
            // unless if controller is not connected...
            else if (!currentState.IsConnected)
            {
                // ...have program complain controller is disconnected
                LinkTextBlock.Text = "Controller: Disconnected";
                LinkTextBlock.Foreground = Brushes.Red;
            }
        }
Esempio n. 10
0
        // controller link dispatcher
        public void Link(object sender, EventArgs dontcare)
        {
            // get state of player one
            currentState = GamePad.GetState(PlayerIndex.One);

            // if controller is connected...
            if (currentState.IsConnected)
            {
                // ...say its connected in the textbloxk, color it green cuz its good to go
                LinkTextBlock.Text       = "Controller: Connected";
                LinkTextBlock.Foreground = Brushes.Green;

                foreach (Buttons b in Enum.GetValues(typeof(Buttons)))
                {
                    Button(b);
                }
                double left_y = currentState.ThumbSticks.Left.Y;
                double left_x = -currentState.ThumbSticks.Left.X;

                if (_adr)
                {
                    //left_x *= -1;
                    left_y *= -1;
                }
                gm.Twist vel = new gm.Twist {
                    linear = new gm.Vector3 {
                        x = left_y * _trans.Value
                    }, angular = new gm.Vector3 {
                        z = left_x * _rot.Value
                    }
                };
                if (velPub != null)
                {
                    velPub.publish(vel);
                }

                //arm controls via joystick are done here.
                double right_y = currentState.ThumbSticks.Right.Y;

                // this is inverted to reflect mikes arm driver.  right requires a negative number, not the default positive value
                double right_x       = -1 * currentState.ThumbSticks.Right.X;
                double right_trigger = currentState.Triggers.Right;

                if (!engaged && (Math.Abs(right_y) > .1 || Math.Abs(right_x) > .1))
                {
                    engaged = true;
                    ArmON.publish(new m.Bool()
                    {
                        data = true
                    });
                    Arm_Engaged.Content    = "Arm Engaged";
                    Arm_Engaged.Background = Brushes.White;
                    Arm_Engaged.Foreground = Brushes.Green;
                }

                //if trigger is not pressed, send close signal ( -1 ).  Th goal is to have the gripper
                // going to a close state when the right trigger is not being pressed.
                if (right_trigger == 0)
                {
                    right_trigger = -1;
                }

                /*Console.WriteLine( "joy_right_x: " + right_x.ToString());
                 * Console.WriteLine( "joy_right_y: " + right_y.ToString());
                 * Console.WriteLine( "right trigger: " + right_trigger.ToString());*/

                am.ArmMovement armmove = new am.ArmMovement();

                armmove.pan_motor_velocity  = right_x;
                armmove.tilt_motor_velocity = right_y;
                armmove.gripper_open        = (right_trigger >= 0.5);

                if (armPub != null)
                {
                    armPub.publish(armmove);
                }
            }
            // unless if controller is not connected...
            else if (!currentState.IsConnected)
            {
                // ...have program complain controller is disconnected
                LinkTextBlock.Text       = "Controller: Disconnected";
                LinkTextBlock.Foreground = Brushes.Red;
            }
        }