Inheritance: OscPacket
Example #1
0
        public void SendOscControlMessage(string address, params object[] values)
        {
            Console.WriteLine("Sending {0}", address);
            var oscMsg = new OscMessage(address, values);

            lock (sendMessages)
            {
                sendMessages[oscMsg.Address] = oscMsg;
            }
        }
Example #2
0
        public void SendOscMessage(string address, double value)
        {
            Console.WriteLine("Sending {0} - {1}", address, value);
            var oscMsg = new OscMessage(address, (float)value);

            lock (sendMessages)
            {
                sendMessages[oscMsg.Address] = oscMsg;
            }
        }
        public positionMessages(indivJoint myJoint)
        {
            register = myJoint.posReg;
            //sender = msend;
            StringBuilder addressRoot = new StringBuilder();
            addressRoot.Append("/");
            addressRoot.Append(myJoint.type.ToString());
            addressRoot.Append("/");
            String root = addressRoot.ToString();

            StringBuilder xAddress = new StringBuilder(root);
            StringBuilder yAddress = new StringBuilder(root);
            StringBuilder zAddress = new StringBuilder(root);

            xAddress.Append("x");
            yAddress.Append("y");
            zAddress.Append("z");

            x_OSC = new OscMessage(xAddress.ToString(), (float)myJoint.loc.X);
            y_OSC = new OscMessage(yAddress.ToString(), (float)myJoint.loc.Y);
            z_OSC = new OscMessage(zAddress.ToString(), (float)myJoint.loc.Z);
        }
        public distanceMessages(jPair myJPair)
        {
            register = myJPair.distReg;
            //messageSender = mSend;
            StringBuilder addressRoot = new StringBuilder();
            addressRoot.Append("/");
            addressRoot.Append(myJPair.jointType0.ToString());
            addressRoot.Append("_");
            addressRoot.Append(myJPair.jointType1.ToString());
            addressRoot.Append("/");
            String root = addressRoot.ToString();

            StringBuilder xAddress = new StringBuilder(root);
            StringBuilder yAddress = new StringBuilder(root);
            StringBuilder zAddress = new StringBuilder(root);
            StringBuilder xyAddress = new StringBuilder(root);
            StringBuilder xzAddress = new StringBuilder(root);
            StringBuilder yzAddress = new StringBuilder(root);
            StringBuilder xyzAddress = new StringBuilder(root);
            StringBuilder angle = new StringBuilder(root);

            xAddress.Append("x");
            yAddress.Append("y");
            zAddress.Append("z");
            xyAddress.Append("xy");
            xzAddress.Append("xz");
            yzAddress.Append("yz");
            xyzAddress.Append("xyz");
            angle.Append("theta");

            xdistOSC = new OscMessage(xAddress.ToString(), (float)myJPair.pair.xdist);
            ydistOSC = new OscMessage(yAddress.ToString(), (float)myJPair.pair.ydist);
            zdistOSC = new OscMessage(zAddress.ToString(), (float)myJPair.pair.zdist);
            xydistOSC = new OscMessage(xyAddress.ToString(), (float)myJPair.pair.xydist);
            xzdistOSC = new OscMessage(xzAddress.ToString(), (float)myJPair.pair.xzdist);
            yzdistOSC = new OscMessage(yzAddress.ToString(), (float)myJPair.pair.yzdist);
            xyzdistOSC = new OscMessage(xyzAddress.ToString(), (float)myJPair.pair.xyzdist);
            angleOSC = new OscMessage(angle.ToString(), (float)myJPair.pair.theta);
        }
        /*
            Sends any unlabled markers out over OSC.
            Address Pattern: \unlabledMarker
            Format: String[4]
                MarkerID
                GlobalMarkerPosition.x
                GlobalMarkerPosition.y
                GlobalMarkerPosition.z
        */
        private static void sendUnlabeledMarkers(Client MyClient, UDPSender sender)
        {
            // Get the unlabeled markers
            uint UnlabeledMarkerCount = MyClient.GetUnlabeledMarkerCount().MarkerCount;
            Console.WriteLine("    Unlabeled Markers ({0}):", UnlabeledMarkerCount);
            for (uint UnlabeledMarkerIndex = 0; UnlabeledMarkerIndex < UnlabeledMarkerCount; ++UnlabeledMarkerIndex)
            {
                // Get the global marker translation
                Output_GetUnlabeledMarkerGlobalTranslation _Output_GetUnlabeledMarkerGlobalTranslation
                  = MyClient.GetUnlabeledMarkerGlobalTranslation(UnlabeledMarkerIndex);

                String[] msg = new String[4];
                msg[0] = "UnlabledMarkerID: " + UnlabeledMarkerIndex + "";
                msg[1] = "pos.x: " + _Output_GetUnlabeledMarkerGlobalTranslation.Translation[0].ToString();
                msg[2] = "pos.y: " + _Output_GetUnlabeledMarkerGlobalTranslation.Translation[1].ToString();
                msg[3] = "pos.z: " + _Output_GetUnlabeledMarkerGlobalTranslation.Translation[2].ToString();

                var message = new SharpOSC.OscMessage("/unlabledMarker", msg);
                sender.Send(message);

                Console.WriteLine("      Marker #{0}: ({1}, {2}, {3})",
                                   UnlabeledMarkerIndex,
                                   _Output_GetUnlabeledMarkerGlobalTranslation.Translation[0],
                                   _Output_GetUnlabeledMarkerGlobalTranslation.Translation[1],
                                   _Output_GetUnlabeledMarkerGlobalTranslation.Translation[2]);
            }
        }
        private static void sendLocalRotationQuaternion(Client MyClient, UDPSender sender)
        {
            // Count the number of subjects
            uint SubjectCount = MyClient.GetSubjectCount().SubjectCount;
            for (uint SubjectIndex = 0; SubjectIndex < SubjectCount; ++SubjectIndex)
            {

                // Get the subject name
                string SubjectName = MyClient.GetSubjectName(SubjectIndex).SubjectName;
                Console.WriteLine("    Name: {0}", SubjectName);

                // Get the root segment
                string RootSegment = MyClient.GetSubjectRootSegmentName(SubjectName).SegmentName;
                Console.WriteLine("    Root Segment: {0}", RootSegment);

                //Get the static segment translation
                Output_GetSegmentLocalRotationQuaternion Output =
                MyClient.GetSegmentLocalRotationQuaternion(SubjectName, RootSegment);

                Console.WriteLine("        LOCAL Rotation Quaternion: ({0},{1},{2},{3})",
                                   Output.Rotation[0],
                                   Output.Rotation[1],
                                   Output.Rotation[2],
                                   Output.Rotation[3]);

                String[] msg = new String[5];
                msg[0] = "RigidBody Name: " + SubjectName;
                msg[1] = "q.x: " + Output.Rotation[0].ToString();
                msg[2] = "q.y: " + Output.Rotation[1].ToString();
                msg[3] = "q.z: " + Output.Rotation[2].ToString();
                msg[4] = "q.w: " + Output.Rotation[3].ToString();

                var message = new SharpOSC.OscMessage("/localQuat", msg);
                sender.Send(message);

            }
        }
        /*
            Sends rigid body data out over OSC.
            Address Pattern: \rigidBody
            Format: String[4]
                RigidBodyName
                GlobalPosition.x
                GlobalPosition.y
                GlobalPosition.z
                GlobalOrientation.qx
                GlobalOrientation.qy
                GlobalOrientation.qz
                GlobalOrientation.qw
        */
        private static void sendRigidBodies(Client MyClient, UDPSender sender)
        {
            // Count the number of subjects
            uint SubjectCount = MyClient.GetSubjectCount().SubjectCount;
            for (uint SubjectIndex = 0; SubjectIndex < SubjectCount; ++SubjectIndex)
            {

                // Get the subject name
                string SubjectName = MyClient.GetSubjectName(SubjectIndex).SubjectName;
                Console.WriteLine("    Name: {0}", SubjectName);

                // Get the root segment
                string RootSegment = MyClient.GetSubjectRootSegmentName(SubjectName).SegmentName;
                Console.WriteLine("    Root Segment: {0}", RootSegment);

                //Get the static segment translation
                Output_GetSegmentGlobalTranslation _Output_GetSegmentGlobalTranslation =
                MyClient.GetSegmentGlobalTranslation(SubjectName, RootSegment);
                Console.WriteLine("        Global Translation: ({0},{1},{2}) {3}",
                                   _Output_GetSegmentGlobalTranslation.Translation[0],
                                   _Output_GetSegmentGlobalTranslation.Translation[1],
                                   _Output_GetSegmentGlobalTranslation.Translation[2],
                                   _Output_GetSegmentGlobalTranslation.Occluded);

                // Get the global segment rotation in quaternion co-ordinates
                Output_GetSegmentGlobalRotationQuaternion _Output_GetSegmentGlobalRotationQuaternion =
                MyClient.GetSegmentGlobalRotationQuaternion(SubjectName, RootSegment);
                Console.WriteLine("        Global Rotation Quaternion: ({0},{1},{2},{3}) {4}",
                                   _Output_GetSegmentGlobalRotationQuaternion.Rotation[0],
                                   _Output_GetSegmentGlobalRotationQuaternion.Rotation[1],
                                   _Output_GetSegmentGlobalRotationQuaternion.Rotation[2],
                                   _Output_GetSegmentGlobalRotationQuaternion.Rotation[3],
                                   _Output_GetSegmentGlobalRotationQuaternion.Occluded);

                String[] msg = new String[8];
                msg[0] = "RigidBody Name: "+ SubjectName;
                msg[1] = "pos.x: " + _Output_GetSegmentGlobalTranslation.Translation[0].ToString();
                msg[2] = "pos.y: " + _Output_GetSegmentGlobalTranslation.Translation[1].ToString();
                msg[3] = "pos.z: " + _Output_GetSegmentGlobalTranslation.Translation[2].ToString();
                msg[4] = "q.x: " + _Output_GetSegmentGlobalRotationQuaternion.Rotation[0].ToString();
                msg[5] = "q.y: " + _Output_GetSegmentGlobalRotationQuaternion.Rotation[1].ToString();
                msg[6] = "q.z: " + _Output_GetSegmentGlobalRotationQuaternion.Rotation[2].ToString();
                msg[7] = "q.w: " + _Output_GetSegmentGlobalRotationQuaternion.Rotation[3].ToString();

                // ignore dropped tracking frames
                if (_Output_GetSegmentGlobalTranslation.Translation[0] != 0 &&
                    _Output_GetSegmentGlobalTranslation.Translation[1] != 0 &&
                    _Output_GetSegmentGlobalTranslation.Translation[2] != 0 ) {
                    var message = new SharpOSC.OscMessage("/rigidBody", msg);
                    sender.Send(message);
                }

            }
        }
        /*
            Sends any labled markers out over OSC.
            Address Pattern: \labledMarker
            Format: String[5]
                MarkerID
                MarkerName
                GlobalMarkerPosition.x
                GlobalMarkerPosition.y
                GlobalMarkerPosition.z
        */
        private static void sendLabledMarkers(Client MyClient, UDPSender sender)
        {
            // For each subject in the scene
            uint SubjectCount = MyClient.GetSubjectCount().SubjectCount;
            for (uint SubjectIndex = 0; SubjectIndex < SubjectCount; ++SubjectIndex)
            {
                // Get the subject name
                string SubjectName = MyClient.GetSubjectName(SubjectIndex).SubjectName;

                // Count the number of markers
                uint MarkerCount = MyClient.GetMarkerCount(SubjectName).MarkerCount;

                // for each marker in subject
                for (uint MarkerIndex = 0; MarkerIndex < MarkerCount; ++MarkerIndex)
                {
                    // Get the marker name
                    string MarkerName = MyClient.GetMarkerName(SubjectName, MarkerIndex).MarkerName;

                    // Get the global marker translation
                    Output_GetMarkerGlobalTranslation _Output_GetMarkerGlobalTranslation =
                      MyClient.GetMarkerGlobalTranslation(SubjectName, MarkerName);

                    String[] msg = new String[5];
                    msg[0] = "RigidBody Name: "+MarkerName;
                    msg[1] = "MarkerID: " + MarkerIndex;
                    msg[2] = "pos.x: " + _Output_GetMarkerGlobalTranslation.Translation[0].ToString();
                    msg[3] = "pos.y: " + _Output_GetMarkerGlobalTranslation.Translation[1].ToString();
                    msg[4] = "pos.z: " + _Output_GetMarkerGlobalTranslation.Translation[2].ToString();

                    // ignore dropped tracking locations
                    if (_Output_GetMarkerGlobalTranslation.Translation[0] != 0 &&
                        _Output_GetMarkerGlobalTranslation.Translation[1] != 0 &&
                        _Output_GetMarkerGlobalTranslation.Translation[2] != 0)
                    {
                        var message = new SharpOSC.OscMessage("/labledMarker", msg);
                        sender.Send(message);
                    }

                    Console.WriteLine("      Marker #{0}: {1} ({2}, {3}, {4}) {5}",
                                       MarkerIndex,
                                       MarkerName,
                                       _Output_GetMarkerGlobalTranslation.Translation[0],
                                       _Output_GetMarkerGlobalTranslation.Translation[1],
                                       _Output_GetMarkerGlobalTranslation.Translation[2],
                                       _Output_GetMarkerGlobalTranslation.Occluded);
               }
            }
        }
Example #9
0
        /// <summary>
        /// Update the spreadsheet.  
        /// Note: This refresh is quite slow and provided here only as a complete example. 
        /// In a production setting this would be optimized.
        /// </summary>
        private static void UpdateData()
        {
            // update MarkerSet data
            for (int i = 0; i < m_FrameOfData.nMarkerSets; i++)
            {
                NatNetML.MarkerSetData ms = m_FrameOfData.MarkerSets[i];
                //for (int j = 0; j < ms.nMarkers; j++)
                //{

                //}
            }

            // update RigidBody data
            Console.WriteLine("RigidBody count: " + m_FrameOfData.nRigidBodies);
            for (int i = 0; i < m_FrameOfData.nRigidBodies; i++)
            {
                NatNetML.RigidBodyData rb = m_FrameOfData.RigidBodies[i];

                // update RigidBody data

                bool tracked = rb.Tracked;
                if (!tracked)
                {
                    //Console.WriteLine("RigidBody not tracked in this frame.");
                }

                ////////////////////////////////////////////////////////////
                // poop

                // send rigid body & markers over OSC
                // may not be fast enough?

                String[] msg = new String[8];
                msg[0] = "Rigid Body: " + rb.ID;
                msg[1] = "x: " + rb.x * 1000;
                msg[2] = "y: " + rb.y * 1000;
                msg[3] = "z: " + rb.z * 1000;
                msg[4] = "qx: " + rb.qx;
                msg[5] = "qy: " + rb.qy;
                msg[6] = "qz: " + rb.qz;
                msg[7] = "qz: " + rb.qw;

                var message = new SharpOSC.OscMessage("/rigid body", msg);
                sender.Send(message);

                // print out to console
                for (int j = 0; j < 8; j++)
                {
                    if (j == 0)
                        Console.WriteLine(msg[j]+":");
                    else
                        Console.WriteLine("     "+msg[j]);
                }

                int count = rb.nMarkers;
                msg = new String[count];
                for (int j = 0; j < count; j++)
                {
                    if (rb.Markers[j] != null)
                        msg[j] = "Rigid Body " + rb.ID + ", Marker " + rb.Markers[j].ID + ": {" + rb.Markers[j].x + "," + rb.Markers[j].y + "," + rb.Markers[j].z + "}";
                }

                sender.Send(new SharpOSC.OscMessage("/marker", msg));

                // print out to console
                for (int j = 0; j < count; j++)
                {
                    if (j == 0)
                        Console.WriteLine(msg[j] + ":");
                    else
                        Console.WriteLine("     " + msg[j]);
                }

                ////////////////////////////////////////////////////////////

                // update Marker data associated with this rigid body
                for (int j = 0; j < rb.nMarkers; j++)
                {
                    if (rb.Markers[j].ID != -1)
                    {
                        RigidBody rbDef = FindRB(rb.ID);
                        if (rbDef != null)
                        {
                            String strUniqueName = rbDef.Name + "-" + rb.Markers[j].ID.ToString();
                            int keyMarker = strUniqueName.GetHashCode();

                            NatNetML.Marker m = rb.Markers[j];

                        }
                    }
                }

            }

            // update Skeleton data
            //for (int i = 0; i < m_FrameOfData.nSkeletons; i++)
            //{
            //    NatNetML.SkeletonData sk = m_FrameOfData.Skeletons[i];
            //    for (int j = 0; j < sk.nRigidBodies; j++)
            //    {
            //        // note : skeleton rigid body ids are of the form:
            //        // parent skeleton ID   : high word (upper 16 bits of int)
            //        // rigid body id        : low word  (lower 16 bits of int)
            //        NatNetML.RigidBodyData rb = sk.RigidBodies[j];
            //        int skeletonID = HighWord(rb.ID);
            //        int rigidBodyID = LowWord(rb.ID);
            //        int uniqueID = skeletonID * 1000 + rigidBodyID;
            //        int key = uniqueID.GetHashCode();
            //        if (htRigidBodies.ContainsKey(key))
            //        {
            //            int rowIndex = (int)htRigidBodies[key];
            //            if (rowIndex >= 0)
            //            {
            //                dataGridView1.Rows[rowIndex].Cells[1].Value = rb.x;
            //                dataGridView1.Rows[rowIndex].Cells[2].Value = rb.y;
            //                dataGridView1.Rows[rowIndex].Cells[3].Value = rb.z;

            //                // Convert quaternion to eulers.  Motive coordinate conventions: X(Pitch), Y(Yaw), Z(Roll), Relative, RHS
            //                float[] quat = new float[4] { rb.qx, rb.qy, rb.qz, rb.qw };
            //                float[] eulers = new float[3];
            //                eulers = m_NatNet.QuatToEuler(quat, (int)NATEulerOrder.NAT_XYZr);
            //                double x = RadiansToDegrees(eulers[0]);     // convert to degrees
            //                double y = RadiansToDegrees(eulers[1]);
            //                double z = RadiansToDegrees(eulers[2]);

            //                dataGridView1.Rows[rowIndex].Cells[4].Value = x;
            //                dataGridView1.Rows[rowIndex].Cells[5].Value = y;
            //                dataGridView1.Rows[rowIndex].Cells[6].Value = z;

            //                // Marker data associated with this rigid body
            //                for (int k = 0; k < rb.nMarkers; k++)
            //                {

            //                }
            //            }
            //        }
            //    }
            //}   // end skeleton update

            //// update labeled markers data
            //// remove previous dynamic marker list
            //// for testing only - this simple approach to grid updating too slow for large marker count use
            //int labeledCount = 0;
            //if (false)
            //{

            //    for (int i = 0; i < m_FrameOfData.nMarkers; i++)
            //    {
            //        NatNetML.Marker m = m_FrameOfData.LabeledMarkers[i];

            //        int modelID, markerID;
            //        m_NatNet.DecodeID(m.ID, out modelID, out markerID);
            //        string name = "Labeled Marker (ModelID: " + modelID + "  MarkerID: " + markerID + ")";
            //        if (modelID == 0)
            //            name = "UnLabeled Marker ( ID: " + markerID + ")";

            //        labeledCount++;
            //    }
            //}
        }
Example #10
0
        internal void ProcessControlMessage(OscMessage msg)
        {
            if (msg.Address == "/Control/PresetData")
            {
                var serializedData = msg.Arguments[0] as string;
                if (serializedData == null)
                    return;

                var data = serializedData
                    .Split(new[] { ';' }, StringSplitOptions.RemoveEmptyEntries)
                    .Select(x => x.Split('='))
                    .Where(x => x.Length == 2)
                    .Select(x => new { Key = x[0], Value = x[1] })
                    .ToDictionary(x => x.Key, x => x.Value);

                SelectedBank = data.GetValueOrDefault("BankName", "Unknown Bank");
                SelectedPreset = data.GetValueOrDefault("PresetName", "Unknown Preset");
            }
            else if(msg.Address == "/Control/ParameterData")
            {
                var module = (Module)(int)msg.Arguments[0];
                var parameter = (int)msg.Arguments[1];
                var floatVal = (float)msg.Arguments[2];
                var formattedString = (string)msg.Arguments[3];
                ProcessParameterUpdate(module, parameter, floatVal, formattedString.Trim());
                RequestVisual(module, parameter);
            }
            else if (msg.Address == "/Control/Waveforms")
            {
                for (int i = 0; i < msg.Arguments.Length - 1; i+=2)
                {
                    var id = (int)msg.Arguments[i];
                    var specifier = ((string)msg.Arguments[i + 1]).Replace("/Wavetables/", "");
                    waveforms[id] = specifier;
                }

                WaveformList = waveforms.ToDictionary(x => x.Key, x => x.Value);
            }
            else if (msg.Address == "/Control/Banks")
            {
                if (msg.Arguments.Length != 1 || !(msg.Arguments[0] is string))
                    return;

                var bankString = (string)msg.Arguments.First();
                var banks = bankString.Split(new[] { ';' }, StringSplitOptions.RemoveEmptyEntries);
                if (banks.Length == 0)
                    return;

                presetBanks.Clear();
                foreach (var bank in banks)
                    presetBanks[bank] = new List<string>();
                NotifyPropertyChanged(nameof(Banks));
            }
            else if (msg.Address == "/Control/Presets")
            {
                if (msg.Arguments.Length != 2 || !(msg.Arguments[0] is string) || !(msg.Arguments[1] is string))
                    return;

                var bankName = (string)msg.Arguments.First();
                var presets = ((string)msg.Arguments.Last()).Split(new[] { ';' }, StringSplitOptions.RemoveEmptyEntries);

                if (!presetBanks.ContainsKey(bankName) || presets.Length == 0)
                    return;

                presetBanks[bankName].Clear();
                foreach (var preset in presets)
                    presetBanks[bankName].Add(preset);

                NotifyPropertyChanged(nameof(Presets));
            }
            else if (msg.Address == "/Control/PresetsChanged")
            {
                if (msg.Arguments.Length != 1 || !(msg.Arguments[0] is string))
                    return;

                var bank = msg.Arguments.First() as string;
                controlManager.SendOscControlMessage("/Control/RequestPresets", bank);
            }
            else if (msg.Address.StartsWith("/Control/Visual"))
            {
                UpdateVisual((byte[])msg.Arguments[0], (int)msg.Arguments[1]);
            }
            else if (msg.Address == "/Control/VoiceState")
            {
                SetVoiceState((byte[])msg.Arguments[0]);
            }
        }
Example #11
0
        public static string receiveMsg(NetworkStream stream, TcpClient tcpClient, UDPSender sender)
        {
            string msgReceived = "";

            // Create a byte array for the available bytes
            byte[] arrayBytesRequest = new byte[tcpClient.Available];
            // Read the bytes from the stream
            int nRead = stream.Read(arrayBytesRequest, 0, arrayBytesRequest.Length);

            if (nRead > 0)
            {
                // Convert the byte array into a string
                string sMsgRequest = ASCIIEncoding.ASCII.GetString(arrayBytesRequest);
                Console.WriteLine("Msg from Robot: " + sMsgRequest);
                string sMsgAnswer = string.Empty;

                // set the key for the key/value pairs
                string key = "";
                if (sMsgRequest.StartsWith("world"))
                    key = "hello";
                else if (sMsgRequest.StartsWith("p"))
                    key = "target";
                else if (sMsgRequest.StartsWith("["))
                    key = "joints";
                else if (sMsgRequest.StartsWith("SET force"))
                    key = "force";
                else if (sMsgRequest.StartsWith("SET tcp force"))
                    key = "tcp force";

                // set the value for the key/value pairs
                if (key.Equals("hello"))
                {
                    Console.WriteLine("Going to home position: hello " + sMsgRequest + "!");
                    // go to home position

                    msgReceived = "going home";
                }
                else if (key.Equals("target"))
                {
                    int start = 2;
                    msgReceived = sMsgRequest.Substring(2, sMsgRequest.Length - 1 - start);

                    string[] coords = msgReceived.Split(',');
                    if (coords.Length == 6)
                    {
                        float x = float.Parse(coords[0]);
                        float y = float.Parse(coords[1]);
                        float z = float.Parse(coords[2]);
                        float rx = float.Parse(coords[3]);
                        float ry = float.Parse(coords[4]);
                        float rz = float.Parse(coords[5]);

                        pose[0] = x;
                        pose[1] = y;
                        pose[2] = z;
                        pose[3] = rx;
                        pose[4] = ry;
                        pose[5] = rz;

                        // send pose over OSC
                        String[] msg = new String[6];
                        msg[0] = "x: " + pose[0];
                        msg[1] = "y: " + pose[1];
                        msg[2] = "z: " + pose[2];
                        msg[3] = "rx: " + pose[3];
                        msg[4] = "ry: " + pose[4];
                        msg[5] = "rz: " + pose[5];
                        var message = new SharpOSC.OscMessage("/pose", msg);
                        sender.Send(message);

                        Console.WriteLine("Robot Pose:  [" + x + ", " + y + ", " + z + ", " + rx + ", " + ry + ", " + rz + "]");
                    }
                    else
                    {
                        msgReceived = "WTF: " + sMsgRequest;
                    }
                }
                else if (key.Equals("joints"))
                {
                    Console.WriteLine("Parsing joint pose: " + sMsgRequest);
                    int start = 1;
                    msgReceived = sMsgRequest.Substring(start, sMsgRequest.Length - 1 - start);

                    string[] coords = msgReceived.Split(',');
                    if (coords.Length == 6)
                    {
                        float x = float.Parse(coords[0]);
                        float y = float.Parse(coords[1]);
                        float z = float.Parse(coords[2]);
                        float rx = float.Parse(coords[3]);
                        float ry = float.Parse(coords[4]);
                        float rz = float.Parse(coords[5]);

                        joints[0] = x;
                        joints[1] = y;
                        joints[2] = z;
                        joints[3] = rx;
                        joints[4] = ry;
                        joints[5] = rz;

                        // send joints over OSC
                        String[] msg = new String[6];
                        msg[0] = "base: " + pose[0];
                        msg[1] = "shoulder: " + pose[1];
                        msg[2] = "elbow: " + pose[2];
                        msg[3] = "wrist 1: " + pose[3];
                        msg[4] = "wrist 2: " + pose[4];
                        msg[5] = "wrist 3: " + pose[5];
                        var message = new SharpOSC.OscMessage("/joints", msg);
                        sender.Send(message);

                        Console.WriteLine("Robot Joint Positions: [" + x + ", " + y + ", " + z + ", " + rx + ", " + ry + ", " + rz + "]");
                    }
                    else
                    {
                        msgReceived = "WTF: " + sMsgRequest;
                    }
                }
                else if (key.Equals("force"))
                {
                    Console.WriteLine("Parsing pose: " + sMsgRequest);
                    int start = key.Length + 5;
                    msgReceived = sMsgRequest.Substring(start, sMsgRequest.Length - 1 - start);

                    force = float.Parse(msgReceived);

                    // send force over OSC
                    var message = new SharpOSC.OscMessage("/force", force);
                    sender.Send(message);

                    Console.WriteLine("Robot Force: {" + force + "}");
                }
                else if (key.Equals("tcp force"))
                {
                    Console.WriteLine("Parsing pose: " + sMsgRequest);
                    int start = key.Length + 5;
                    msgReceived = sMsgRequest.Substring(start, sMsgRequest.Length - 1 - start);

                    tcpForce = float.Parse(msgReceived);

                    // send TCP force over OSC
                    var message = new SharpOSC.OscMessage("/TCPforce", tcpForce);
                    sender.Send(message);

                    Console.WriteLine("Robot TCP Force: {" + tcpForce + "}");
                }
                else
                {
                    // unkown command
                    msgReceived = sMsgRequest;
                    ///sendMsg(stream, "unknown command sent to robot: " + sMsgRequest);
                }

            }
            else if (tcpClient.Available == 0)
            {
                Console.WriteLine("Client closed the connection.");
                // No bytes read, and no bytes available, the client is closed.
                stream.Close();
            }

            return msgReceived;
        }
Example #12
0
 private void TransmitData(VideoPacket packet)
 {
     var message = new OscMessage("/test/1", ObjectToString(packet));
     var udpSender = new UDPSender("127.0.0.1", 55555);
     udpSender.Send(message);
 }