/// <summary>
        /// [NatNet] Request a description of the Active Model List from the server (e.g. Motive) and build up a new spreadsheet
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void buttonGetDataDescriptions_Click(object sender, EventArgs e)
        {
            mRigidBodies.Clear();
            dataGridView1.Rows.Clear();
            htMarkers.Clear();
            htRigidBodies.Clear();
            needMarkerListUpdate = true;

            OutputMessage("Retrieving Data Descriptions....");
            List <NatNetML.DataDescriptor> descs = new List <NatNetML.DataDescriptor>();
            bool bSuccess = m_NatNet.GetDataDescriptions(out descs);

            if (bSuccess)
            {
                OutputMessage(String.Format("Retrieved {0} Data Descriptions....", descs.Count));
                int iObject = 0;
                foreach (NatNetML.DataDescriptor d in descs)
                {
                    iObject++;

                    // MarkerSets
                    if (d.type == (int)NatNetML.DataDescriptorType.eMarkerSetData)
                    {
                        NatNetML.MarkerSet ms = (NatNetML.MarkerSet)d;
                        OutputMessage("Data Def " + iObject.ToString() + " [MarkerSet]");

                        OutputMessage(" Name : " + ms.Name);
                        OutputMessage(String.Format(" Markers ({0}) ", ms.nMarkers));
                        dataGridView1.Rows.Add("MarkerSet: " + ms.Name);
                        for (int i = 0; i < ms.nMarkers; i++)
                        {
                            OutputMessage(("  " + ms.MarkerNames[i]));
                            int rowIndex = dataGridView1.Rows.Add("  " + ms.MarkerNames[i]);
                            // MarkerNameIndexToRow map
                            String strUniqueName = ms.Name + i.ToString();
                            int    key           = strUniqueName.GetHashCode();
                            htMarkers.Add(key, rowIndex);
                        }
                    }
                    // RigidBodies
                    else if (d.type == (int)NatNetML.DataDescriptorType.eRigidbodyData)
                    {
                        NatNetML.RigidBody rb = (NatNetML.RigidBody)d;

                        OutputMessage("Data Def " + iObject.ToString() + " [RigidBody]");
                        OutputMessage(" Name : " + rb.Name);
                        OutputMessage(" ID : " + rb.ID);
                        OutputMessage(" ParentID : " + rb.parentID);
                        OutputMessage(" OffsetX : " + rb.offsetx);
                        OutputMessage(" OffsetY : " + rb.offsety);
                        OutputMessage(" OffsetZ : " + rb.offsetz);

                        mRigidBodies.Add(rb);

                        int rowIndex = dataGridView1.Rows.Add("RigidBody: " + rb.Name);
                        // RigidBodyIDToRow map
                        int key = rb.ID.GetHashCode();
                        htRigidBodies.Add(key, rowIndex);
                    }
                    // Skeletons
                    else if (d.type == (int)NatNetML.DataDescriptorType.eSkeletonData)
                    {
                        NatNetML.Skeleton sk = (NatNetML.Skeleton)d;

                        OutputMessage("Data Def " + iObject.ToString() + " [Skeleton]");
                        OutputMessage(" Name : " + sk.Name);
                        OutputMessage(" ID : " + sk.ID);
                        dataGridView1.Rows.Add("Skeleton: " + sk.Name);
                        for (int i = 0; i < sk.nRigidBodies; i++)
                        {
                            RigidBody rb = sk.RigidBodies[i];
                            OutputMessage(" RB Name  : " + rb.Name);
                            OutputMessage(" RB ID    : " + rb.ID);
                            OutputMessage(" ParentID : " + rb.parentID);
                            OutputMessage(" OffsetX  : " + rb.offsetx);
                            OutputMessage(" OffsetY  : " + rb.offsety);
                            OutputMessage(" OffsetZ  : " + rb.offsetz);
                            int rowIndex = dataGridView1.Rows.Add("Bone: " + rb.Name);
                            // RigidBodyIDToRow map
                            int uniqueID = sk.ID * 1000 + rb.ID;
                            int key      = uniqueID.GetHashCode();
                            if (htRigidBodies.ContainsKey(key))
                            {
                                MessageBox.Show("Duplicate RigidBody ID");
                            }
                            else
                            {
                                htRigidBodies.Add(key, rowIndex);
                            }
                        }
                    }
                    else
                    {
                        OutputMessage("Unknown DataType");
                    }
                }
            }
            else
            {
                OutputMessage("Unable to retrieve DataDescriptions");
            }
        }
示例#2
0
        static void parseDataDescriptor(List <NatNetML.DataDescriptor> description)
        {
            //  [NatNet] Request a description of the Active Model List from the server.
            //  This sample will list only names of the data sets, but you can access
            int numDataSet = description.Count;

            Console.WriteLine("Total {0} data sets in the capture:", numDataSet);

            for (int i = 0; i < numDataSet; ++i)
            {
                int dataSetType = description[i].type;
                // Parse Data Descriptions for each data sets and save them in the delcared lists and hashtables for later uses.
                switch (dataSetType)
                {
                case ((int)NatNetML.DataDescriptorType.eMarkerSetData):
                    NatNetML.MarkerSet mkset = (NatNetML.MarkerSet)description[i];
                    Console.WriteLine("\tMarkerSet ({0})", mkset.Name);
                    break;


                case ((int)NatNetML.DataDescriptorType.eRigidbodyData):
                    NatNetML.RigidBody rb = (NatNetML.RigidBody)description[i];
                    Console.WriteLine("\tRigidBody ({0})", rb.Name);

                    // Saving Rigid Body Descriptions
                    mRigidBodies.Add(rb);
                    break;


                case ((int)NatNetML.DataDescriptorType.eSkeletonData):
                    NatNetML.Skeleton skl = (NatNetML.Skeleton)description[i];
                    Console.WriteLine("\tSkeleton ({0}), Bones:", skl.Name);

                    //Saving Skeleton Descriptions
                    mSkeletons.Add(skl);

                    // Saving Individual Bone Descriptions
                    for (int j = 0; j < skl.nRigidBodies; j++)
                    {
                        Console.WriteLine("\t\t{0}. {1}", j + 1, skl.RigidBodies[j].Name);
                        int uniqueID = skl.ID * 1000 + skl.RigidBodies[j].ID;
                        int key      = uniqueID.GetHashCode();
                        mHtSkelRBs.Add(key, skl.RigidBodies[j]);     //Saving the bone segments onto the hashtable
                    }
                    break;


                case ((int)NatNetML.DataDescriptorType.eForcePlateData):
                    NatNetML.ForcePlate fp = (NatNetML.ForcePlate)description[i];
                    Console.WriteLine("\tForcePlate ({0})", fp.Serial);

                    // Saving Force Plate Channel Names
                    mForcePlates.Add(fp);

                    for (int j = 0; j < fp.ChannelCount; j++)
                    {
                        Console.WriteLine("\t\tChannel {0}: {1}", j + 1, fp.ChannelNames[j]);
                    }
                    break;

                default:
                    // When a Data Set does not match any of the descriptions provided by the SDK.
                    Console.WriteLine("\tError: Invalid Data Set");
                    break;
                }
            }
        }
示例#3
0
        static void processFrameData(NatNetML.FrameOfMocapData data)
        {
            /*  Parsing Rigid Body Frame Data   */
            for (int i = 0; i < mRigidBodies.Count; i++)
            {
                int rbID = mRigidBodies[i].ID;              // Fetching rigid body IDs from the saved descriptions

                for (int j = 0; j < data.nRigidBodies; j++)
                {
                    if (rbID == data.RigidBodies[j].ID)                      // When rigid body ID of the descriptions matches rigid body ID of the frame data.
                    {
                        NatNetML.RigidBody     rb     = mRigidBodies[i];     // Saved rigid body descriptions
                        NatNetML.RigidBodyData rbData = data.RigidBodies[j]; // Received rigid body descriptions

                        if (rbData.Tracked == true)
                        {
                            Console.WriteLine("\tRigidBody ({0}):", rb.Name);
                            Console.WriteLine("\t\tpos ({0:N3}, {1:N3}, {2:N3})", rbData.x, rbData.y, rbData.z);

                            // Rigid Body Euler Orientation
                            float[] quat = new float[4] {
                                rbData.qx, rbData.qy, rbData.qz, rbData.qw
                            };
                            float[] eulers = new float[3];

                            eulers = NatNetClientML.QuatToEuler(quat, NATEulerOrder.NAT_XYZr); //Converting quat orientation into XYZ Euler representation.
                            double xrot = RadiansToDegrees(eulers[0]);
                            double yrot = RadiansToDegrees(eulers[1]);
                            double zrot = RadiansToDegrees(eulers[2]);

                            Console.WriteLine("\t\tori ({0:N3}, {1:N3}, {2:N3})", xrot, yrot, zrot);
                        }
                        else
                        {
                            Console.WriteLine("\t{0} is not tracked in current frame", rb.Name);
                        }
                    }
                }
            }

            /* Parsing Skeleton Frame Data  */
            for (int i = 0; i < mSkeletons.Count; i++)      // Fetching skeleton IDs from the saved descriptions
            {
                int sklID = mSkeletons[i].ID;

                for (int j = 0; j < data.nSkeletons; j++)
                {
                    if (sklID == data.Skeletons[j].ID)                     // When skeleton ID of the description matches skeleton ID of the frame data.
                    {
                        NatNetML.Skeleton     skl     = mSkeletons[i];     // Saved skeleton descriptions
                        NatNetML.SkeletonData sklData = data.Skeletons[j]; // Received skeleton frame data

                        Console.WriteLine("\tSkeleton ({0}):", skl.Name);
                        Console.WriteLine("\t\tSegment count: {0}", sklData.nRigidBodies);

                        /*  Now, for each of the skeleton segments  */
                        for (int k = 0; k < sklData.nRigidBodies; k++)
                        {
                            NatNetML.RigidBodyData boneData = sklData.RigidBodies[k];

                            /*  Decoding skeleton bone ID   */
                            int skeletonID  = HighWord(boneData.ID);
                            int rigidBodyID = LowWord(boneData.ID);
                            int uniqueID    = skeletonID * 1000 + rigidBodyID;
                            int key         = uniqueID.GetHashCode();

                            NatNetML.RigidBody bone = (RigidBody)mHtSkelRBs[key];   //Fetching saved skeleton bone descriptions

                            //Outputting only the hip segment data for the purpose of this sample.
                            if (k == 0)
                            {
                                Console.WriteLine("\t\t{0:N3}: pos({1:N3}, {2:N3}, {3:N3})", bone.Name, boneData.x, boneData.y, boneData.z);
                            }
                        }
                    }
                }
            }

            /*  Parsing Force Plate Frame Data  */
            for (int i = 0; i < mForcePlates.Count; i++)
            {
                int fpID = mForcePlates[i].ID;                  // Fetching force plate IDs from the saved descriptions

                for (int j = 0; j < data.nForcePlates; j++)
                {
                    if (fpID == data.ForcePlates[j].ID)                       // When force plate ID of the descriptions matches force plate ID of the frame data.
                    {
                        NatNetML.ForcePlate     fp     = mForcePlates[i];     // Saved force plate descriptions
                        NatNetML.ForcePlateData fpData = data.ForcePlates[i]; // Received forceplate frame data

                        Console.WriteLine("\tForce Plate ({0}):", fp.Serial);

                        // Here we will be printing out only the first force plate "subsample" (index 0) that was collected with the mocap frame.
                        for (int k = 0; k < fpData.nChannels; k++)
                        {
                            Console.WriteLine("\t\tChannel {0}: {1}", fp.ChannelNames[k], fpData.ChannelData[k].Values[0]);
                        }
                    }
                }
            }
            Console.WriteLine("\n");
        }
        //static uint GPS_LEAPSECONDS_MILLIS = 18000;

        static void processFrameData(NatNetML.FrameOfMocapData data)
        {
            bool data_gogo = true;

            /*  Parsing Rigid Body Frame Data   */
            for (int i = 0; i < mRigidBodies.Count; i++)
            {
                int rbID = mRigidBodies[i].ID;              // Fetching rigid body IDs from the saved descriptions

                for (int j = 0; j < data.nRigidBodies; j++)
                {
                    if (rbID == data.RigidBodies[j].ID)                      // When rigid body ID of the descriptions matches rigid body ID of the frame data.
                    {
                        NatNetML.RigidBody     rb     = mRigidBodies[i];     // Saved rigid body descriptions
                        NatNetML.RigidBodyData rbData = data.RigidBodies[j]; // Received rigid body descriptions

                        if (rbData.Tracked == true)
                        {
#if DEBUG_MSG
                            Console.WriteLine("\tRigidBody ({0}):", rb.Name);
                            Console.WriteLine("\t\tpos ({0:N3}, {1:N3}, {2:N3})", rbData.x, rbData.y, rbData.z);

                            // Rigid Body Euler Orientation
                            float[] quat = new float[4] {
                                rbData.qx, rbData.qy, rbData.qz, rbData.qw
                            };
                            float[] eulers = new float[3];

                            eulers = NatNetClientML.QuatToEuler(quat, NATEulerOrder.NAT_XYZr); //Converting quat orientation into XYZ Euler representation.
                            double xrot = RadiansToDegrees(eulers[0]);
                            double yrot = RadiansToDegrees(eulers[1]);
                            double zrot = RadiansToDegrees(eulers[2]);
                            Console.WriteLine("\t\tori ({0:N3}, {1:N3}, {2:N3})", xrot, yrot, zrot);
#endif
                            if (drones.ContainsKey(rb.Name))
                            {
                                DroneData drone = drones[rb.Name];
                                drone.lost_count = 0;
                                long cur_ms = stopwatch.ElapsedMilliseconds;
                                if (drone.send_count > 0)
                                {
                                    drone.send_count--;
                                }
                                else if (data_gogo)
                                {
                                    drone.send_count = 10;
                                    data_gogo        = false;
                                    MAVLink.mavlink_att_pos_mocap_t att_pos = new MAVLink.mavlink_att_pos_mocap_t();
                                    att_pos.time_usec = (ulong)(cur_ms * 1000);
                                    att_pos.x         = rbData.x;  //north
                                    att_pos.y         = rbData.z;  //east
                                    att_pos.z         = -rbData.y; //down
                                    att_pos.q         = new float[4] {
                                        rbData.qw, rbData.qx, rbData.qz, -rbData.qy
                                    };
                                    byte[] pkt = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.ATT_POS_MOCAP, att_pos);

                                    if (drone.lastTime >= 0)
                                    {
                                        MAVLink.mavlink_vision_speed_estimate_t vis_speed = new MAVLink.mavlink_vision_speed_estimate_t();
                                        float total_s = (cur_ms - drone.lastTime) * 0.001f;
                                        vis_speed.x    = (rbData.x - drone.lastPosN) / total_s;
                                        vis_speed.y    = (rbData.z - drone.lastPosE) / total_s;
                                        vis_speed.z    = (-rbData.y - drone.lastPosD) / total_s;
                                        vis_speed.usec = (ulong)(cur_ms * 1000);
                                        byte[] pkt2      = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.VISION_SPEED_ESTIMATE, vis_speed);
                                        int    total_len = pkt.Length + pkt2.Length;
                                        byte[] uwb_data  = new byte[10 + total_len + 1];
                                        uwb_data[0] = 0x54;
                                        uwb_data[1] = 0xf1;
                                        uwb_data[2] = 0xff;
                                        uwb_data[3] = 0xff;
                                        uwb_data[4] = 0xff;
                                        uwb_data[5] = 0xff;
                                        uwb_data[6] = 2;
                                        uwb_data[7] = drone.uwb_tag_id;
                                        uwb_data[8] = (byte)(total_len & 0xff);
                                        uwb_data[9] = (byte)(total_len >> 16);
                                        Array.Copy(pkt, 0, uwb_data, 10, pkt.Length);
                                        Array.Copy(pkt2, 0, uwb_data, 10 + pkt.Length, pkt2.Length);
                                        byte chk_sum = 0;
                                        foreach (byte uwb_data_byte in uwb_data)
                                        {
                                            chk_sum += uwb_data_byte;
                                        }
                                        uwb_data[uwb_data.Length - 1] = chk_sum;
                                        mavSock.SendTo(uwb_data, drone.ep);
                                    }
                                }
                                drone.lastTime = cur_ms;
                                drone.lastPosN = rbData.x;
                                drone.lastPosE = rbData.z;
                                drone.lastPosD = -rbData.y;
                            }
                        }
                        else
                        {
                            Console.WriteLine("\t{0} is not tracked in current frame", rb.Name);

                            /*if (drones.ContainsKey(rb.Name))
                             * {
                             *  DroneData drone = drones[rb.Name];
                             *  drone.lost_count++;
                             *  if (drone.lost_count > 3)
                             *  {
                             *      drone.lost_count = 0;
                             *      MAVLink.mavlink_gps_input_t gps_input = new MAVLink.mavlink_gps_input_t();
                             *      gps_input.gps_id = 0;
                             *      gps_input.fix_type = (byte)MAVLink.GPS_FIX_TYPE.NO_FIX;
                             *      byte[] pkt = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.GPS_INPUT, gps_input);
                             *      mavSock.SendTo(pkt, drone.ep);
                             *  }
                             * }*/
                        }
                    }
                }
            }

            /* Parsing Skeleton Frame Data  */
            for (int i = 0; i < mSkeletons.Count; i++)      // Fetching skeleton IDs from the saved descriptions
            {
                int sklID = mSkeletons[i].ID;

                for (int j = 0; j < data.nSkeletons; j++)
                {
                    if (sklID == data.Skeletons[j].ID)                     // When skeleton ID of the description matches skeleton ID of the frame data.
                    {
                        NatNetML.Skeleton     skl     = mSkeletons[i];     // Saved skeleton descriptions
                        NatNetML.SkeletonData sklData = data.Skeletons[j]; // Received skeleton frame data

                        Console.WriteLine("\tSkeleton ({0}):", skl.Name);
                        Console.WriteLine("\t\tSegment count: {0}", sklData.nRigidBodies);

                        /*  Now, for each of the skeleton segments  */
                        for (int k = 0; k < sklData.nRigidBodies; k++)
                        {
                            NatNetML.RigidBodyData boneData = sklData.RigidBodies[k];

                            /*  Decoding skeleton bone ID   */
                            int skeletonID  = HighWord(boneData.ID);
                            int rigidBodyID = LowWord(boneData.ID);
                            int uniqueID    = skeletonID * 1000 + rigidBodyID;
                            int key         = uniqueID.GetHashCode();

                            NatNetML.RigidBody bone = (RigidBody)mHtSkelRBs[key];   //Fetching saved skeleton bone descriptions

                            //Outputting only the hip segment data for the purpose of this sample.
                            if (k == 0)
                            {
                                Console.WriteLine("\t\t{0:N3}: pos({1:N3}, {2:N3}, {3:N3})", bone.Name, boneData.x, boneData.y, boneData.z);
                            }
                        }
                    }
                }
            }

            /*  Parsing Force Plate Frame Data  */
            for (int i = 0; i < mForcePlates.Count; i++)
            {
                int fpID = mForcePlates[i].ID;                  // Fetching force plate IDs from the saved descriptions

                for (int j = 0; j < data.nForcePlates; j++)
                {
                    if (fpID == data.ForcePlates[j].ID)                       // When force plate ID of the descriptions matches force plate ID of the frame data.
                    {
                        NatNetML.ForcePlate     fp     = mForcePlates[i];     // Saved force plate descriptions
                        NatNetML.ForcePlateData fpData = data.ForcePlates[i]; // Received forceplate frame data

                        Console.WriteLine("\tForce Plate ({0}):", fp.Serial);

                        // Here we will be printing out only the first force plate "subsample" (index 0) that was collected with the mocap frame.
                        for (int k = 0; k < fpData.nChannels; k++)
                        {
                            Console.WriteLine("\t\tChannel {0}: {1}", fp.ChannelNames[k], fpData.ChannelData[k].Values[0]);
                        }
                    }
                }
            }
#if DEBUG_MSG
            Console.WriteLine("\n");
#endif
        }