public void Test()
{
var message = new SharpOSC.OscMessage(Address, Msg);
var sender = new SharpOSC.UDPSender("127.0.0.1", 53001);
sender.Send(message);
}
public Form1()
{
InitializeComponent();
//System.Threading.Thread t;
//Run = true;
//t = new System.Threading.Thread(DoThisAllTheTime);
//t.Start();
//int x = Convert.ToInt32(textBox1.Text);
//textBox1.Text = Properties.Settings.Default.portIn.ToString();
//textBox2.Text = Properties.Settings.Default.portOut.ToString();
//textBox3.Text = Properties.Settings.Default.broadcastIP.ToString();
vm = new VmClient();
IPAddress broadcast;
//Console.WriteLine(Properties.Settings.Default.portIn.ToString());
//Console.WriteLine(Properties.Settings.Default.portOut.ToString());
//Console.WriteLine(Properties.Settings.Default.broadcastIP.ToString());
bool flag = IPAddress.TryParse(Properties.Settings.Default.broadcastIP, out broadcast);
if (!flag)
{
Properties.Settings.Default.broadcastIP = "192.168.0.255";
textBox3.Text = "192.168.0.255";
Properties.Settings.Default.Save();
broadcast = IPAddress.Parse(Properties.Settings.Default.broadcastIP);
}
oscSender = new SharpOSC.UDPSender(broadcast.ToString(), Properties.Settings.Default.portOut);
}
public static void init()
{
sender = new SharpOSC.UDPSender("192.168.1.100", 1337);
SendLog("OSC hello");
Debug.Log("OSCSender started.");
}
// Use this for initialization
void Start()
{
var message = new SharpOSC.OscMessage("/note", 60, 100, 1000);
var sender = new SharpOSC.UDPSender("127.0.0.1", 9000);
sender.Send(message);
}
public void sendOscMessage(string mess)
{
Debug.Log("sendOscMessage");
var message = new SharpOSC.OscMessage(mess);
var sender = new SharpOSC.UDPSender("127.0.0.1", 9000);
sender.Send(message);
}
public override void OnFrame(Controller controller)
{
Frame currentFrame = controller.Frame();
currentTime = currentFrame.Timestamp;
timeChanged = currentTime - previousTime;
if (timeChanged > 1000)
{
if (!currentFrame.Hands.IsEmpty)
{
Finger finger = currentFrame.Fingers[0];
Screen screen = controller.LocatedScreens.ClosestScreenHit(finger);
if (screen != null && screen.IsValid)
{
var tipVelocity = ( int )finger.TipVelocity.Magnitude;
//if ( tipVelocity > 25 )
//{
var xScreenIntersect = screen.Intersect(finger, true).x;
var yScreenIntersect = screen.Intersect(finger, true).y;
if (xScreenIntersect.ToString() != "NaN")
{
var x = ( int )(xScreenIntersect * screen.WidthPixels);
var y = ( int )(screen.HeightPixels - (yScreenIntersect * screen.HeightPixels));
Console.WriteLine("Screen intersect X: " + xScreenIntersect.ToString());
Console.WriteLine("Screen intersect Y: " + yScreenIntersect.ToString());
Console.WriteLine("Width pixels: " + screen.WidthPixels.ToString());
Console.WriteLine("Height pixels: " + screen.HeightPixels.ToString());
Console.WriteLine("\n");
Console.WriteLine("x: " + x.ToString());
Console.WriteLine("y: " + y.ToString());
Console.WriteLine("\n");
Console.WriteLine("Tip velocity: " + tipVelocity.ToString());
var message = new SharpOSC.OscMessage("/mouse", x, y, tipVelocity);
var sender = new SharpOSC.UDPSender("127.0.0.1", 55555);
sender.Send(message);
//MouseCursor.MoveCursor( x, y );
Console.WriteLine("\n" + new String('=', 40) + "\n");
}
//}
}
}
}
previousTime = currentTime;
}
public void sendMessages(UDPSender sender)
{
if (register.xPos_status)
{
sender.Send(x_OSC);
}
if (register.yPos_status)
{
sender.Send(y_OSC);
}
if (register.zPos_status)
{
sender.Send(z_OSC);
}
}
void RespondToNewListener(List <object> oscValues)
{
// the listener is now broadcasting to us.
// we should send to IT too.
// get the listener's IP address
string listenerIP = (string)oscValues[0];
if (listenerIP == "")
{
// sadness. what to do?
return;
}
// add it to the list of people we send to
mySenders[listenerIP] = new SharpOSC.UDPSender(listenerIP, OSCPort);
}
public Form1()
{
InitializeComponent();
//System.Threading.Thread t;
//Run = true;
//t = new System.Threading.Thread(DoThisAllTheTime);
//t.Start();
//int x = Convert.ToInt32(textBox1.Text);
textBox1.Text = Properties.Settings.Default.portIn.ToString();
textBox2.Text = Properties.Settings.Default.portOut.ToString();
vm = new VmClient();
IPAddress broadcast = IPAddress.Parse("10.0.3.255");
oscSender = new SharpOSC.UDPSender(broadcast.ToString(), Properties.Settings.Default.portOut);
}
public void sendMessages(UDPSender messageSender)
{
if (register.x_distance_status)
{
messageSender.Send(xdistOSC);
}
if (register.y_distance_status)
{
messageSender.Send(ydistOSC);
}
if (register.z_distance_status)
{
messageSender.Send(zdistOSC);
}
if (register.xy_distance_status)
{
messageSender.Send(xydistOSC);
}
if (register.xz_distance_status)
{
messageSender.Send(xzdistOSC);
}
if (register.yz_distance_status)
{
messageSender.Send(yzdistOSC);
}
if (register.xyz_distance_status)
{
messageSender.Send(xyzdistOSC);
}
if (register.theta_status)
{
messageSender.Send(angleOSC);
}
}
void InitiateConnection()
{
// initialize the sender: easy!
mySender = new SharpOSC.UDPSender(myAddress.Address.ToString(), myAddress.Port);
// initialize the listener: create a callback to route messages
SharpOSC.HandleOscPacket listenerCallback = delegate(SharpOSC.OscPacket packet)
{
// get message
SharpOSC.OscMessage messageReceived = (SharpOSC.OscMessage)packet;
// send message along to be processed on the main thread in Update()
myOSCIncomingMessages.Enqueue(Tuple.Create(messageReceived.Address, messageReceived.Arguments));
};
// initialize the listener
myListener = new SharpOSC.UDPListener(myAddress.Port, listenerCallback);
// tell the other side we exist
SendMessage("/___broadcastToMePlease___", GetMyIP());
}
// Function to send Msg to Linux
void sendPos(GameObject pos_arr)
{
// x and z for plane, y is height
float pos_x = pos_arr.GetComponent <Transform> ().position.x;
float pos_y = pos_arr.GetComponent <Transform> ().position.y;
float pos_z = pos_arr.GetComponent <Transform> ().position.z;
float azi;
float ele;
//Debug.Log(pos_x);
//Debug.Log(pos_y);
//Debug.Log(pos_z);
azi = Mathf.Atan2(pos_x, pos_z) * 180 / Mathf.PI;
ele = Mathf.Atan2(pos_y - 1.6f, Mathf.Sqrt(pos_x * pos_x + pos_z * pos_z)) * 180 / Mathf.PI;
var msg_azi = new SharpOSC.OscMessage("/0x00/azi", azi);
var msg_ele = new SharpOSC.OscMessage("/0x00/ele", ele);
var sender = new SharpOSC.UDPSender(IP_add, port);
sender.Send(msg_azi);
sender.Send(msg_ele);
}
public string open(Dictionary <string, object> parameters)
{
try
{
string host = (string)parameters["host"];
int port = int.Parse((string)parameters["port"]);
if (library == OSCLibrary.SharpOSC)
{
sharposcclient = new SharpOSC.UDPSender(host, port);
}
else
{
ventuzclient = new Ventuz.OSC.UdpWriter(host, port);
}
}
catch (Exception ex)
{
return(ex.ToString());
}
return(null);
}
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;
}
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);
}
// Update is called once per frame
void Update()
{
Frame frame = controller.Frame();
Debug.Log("Frame Id: " + frame.Id);
frameCount++;
HandList hands = frame.Hands;
for (int i = 0; i < hands.Count; i++)
{
Hand hand = hands[i];
Leap.Vector direction = hand.Direction;
Leap.Vector palmNormal = hand.PalmNormal;
Leap.Vector palmPosition = hand.PalmPosition;
Leap.Vector handSpeed = hand.PalmVelocity;
if (i == 0)
{
fdistanceDirection = direction.DistanceTo(fprevDirection);
fdistancePalmPosition = palmPosition.DistanceTo(fprevPalmPosition);
fdistancePalmNormal = palmNormal.DistanceTo(fprevPalmNormal);
fdistanceHandVelocity = handSpeed.DistanceTo(fprevHandVelocity);
fprevDirection = direction;
fprevPalmNormal = palmNormal;
fprevPalmPosition = palmPosition;
fprevHandVelocity = handSpeed;
}
else if (i == 1)
{
sdistanceDirection = direction.DistanceTo(sprevDirection);
sdistancePalmPosition = palmPosition.DistanceTo(sprevPalmPosition);
sdistancePalmNormal = palmNormal.DistanceTo(sprevPalmNormal);
sdistanceHandVelocity = handSpeed.DistanceTo(sprevHandVelocity);
sprevDirection = direction;
sprevPalmNormal = palmNormal;
sprevPalmPosition = palmPosition;
sprevHandVelocity = handSpeed;
Debug.Log("first direction " + fdistanceDirection);
Debug.Log("second direction " + sdistanceDirection);
Debug.Log("Direction change = " + (fdistanceDirection - sdistanceDirection));
directionFile.WriteLine("");
directionFile.WriteLine("frame id: " + frame.Id);
directionFile.WriteLine("Direction first: " + fdistanceDirection + " second: " + sdistanceDirection + " diff: " + (fdistanceDirection - sdistanceDirection));
directionFile.WriteLine("PalmPosition first: " + fdistancePalmPosition + " second: " + sdistancePalmPosition + " diff: " + (fdistancePalmPosition - sdistancePalmPosition));
directionFile.WriteLine("PalmNormal first: " + fdistancePalmNormal + " second: " + sdistancePalmNormal + " diff: " + (fdistancePalmNormal - sdistancePalmNormal));
directionFile.WriteLine("Velocity first: " + fdistanceHandVelocity + " second: " + sdistanceHandVelocity + " diff: " + (fdistanceHandVelocity - sdistanceHandVelocity));
directionFile.WriteLine("");
float directionChange = (fdistanceDirection - sdistanceDirection);
float palmPositionChange = fdistancePalmPosition - sdistancePalmPosition;
float palmNormalChange = fdistancePalmNormal - sdistancePalmNormal;
float velocityChange = fdistanceHandVelocity - sdistanceHandVelocity;
if (directionChange < 0.0f)
{
directionChange *= -1.0f;
}
if (palmPositionChange < 0.0f)
{
palmPositionChange *= -1.0f;
}
if (palmNormalChange < 0.0f)
{
palmNormalChange *= -1.0f;
}
if (velocityChange < 0.0f)
{
velocityChange *= -1.0f;
}
totalDistance += directionChange;
totalPalmPosition += palmPositionChange;
totalPalmNormal += palmNormalChange;
totalVelocity += velocityChange;
test_status = "Status:\n";
if (directionChange < 0.001)
{
test_status += "Direction OK\n";
}
else
{
test_status += "Fix Direction\n";
}
if (palmPositionChange < 1.0)
{
test_status += "PalmPosition OK\n";
}
else
{
test_status += "Fix PalmPosition\n";
}
if (palmNormalChange < 1.0)
{
test_status += "PalmNormal OK\n";
}
else
{
test_status += "Fix PalmNormal\n";
}
if (velocityChange < 1.0)
{
test_status += "Velocity OK\n";
}
else
{
test_status += "Fix Velocity\n";
}
float error = totalDistance * 2.0f + totalPalmNormal * 2.0f + totalVelocity + totalPalmPosition * 3.0f;
error /= (4 * frameCount);
var message = new SharpOSC.OscMessage("/hand/1", error);
var sender = new SharpOSC.UDPSender("127.0.0.1", 55555);
sender.Send(message);
if (error > 5.0)
{
status_text.color = Color.red;
test_status += "Not matching";
//source.PlayOneShot(buzzSound);
}
else
{
status_text.color = Color.green;
test_status += "Matching";
//source.PlayOneShot(matchSound);
}
status_text.text = test_status;
}
}
}
static UDPSender instantiateNewUDPStream(string destinationIP, int destinationPort)
{
UDPSender sender = new SharpOSC.UDPSender(destinationIP, destinationPort);
return(sender);
}
static void Main(string[] args)
{
// OSC message/bundle callback handler.
// In this example it just outputs all the sent responses and arguments to the console.
HandleOscPacket packetCallback = delegate(OscPacket packet)
{
if (packet is OscMessage)
{
var message = packet as OscMessage;
StringBuilder builder = new StringBuilder();
builder.AppendLine(message.Address);
foreach (var argument in message.Arguments)
{
builder.AppendFormat(" {0} ", argument);
}
builder.AppendLine();
Console.WriteLine(builder.ToString());
}
else if (packet is OscBundle)
{
StringBuilder builder = new StringBuilder();
var bundle = packet as OscBundle;
foreach (var message in bundle.Messages)
{
builder.AppendLine(message.Address);
foreach (var argument in message.Arguments)
{
builder.AppendFormat(" {0} ", argument);
}
builder.AppendLine();
Console.WriteLine(builder.ToString());
}
}
};
// Create a OSC message listener object, lets print out all qtm server replies in a callback method
var listener = new UDPListener(55555, packetCallback);
// Create a OSC message sender object
var sender = new SharpOSC.UDPSender("127.0.0.1", 22225);
// Connect
sender.Send(new SharpOSC.OscMessage("/qtm", "connect", 55555));
// Ask for QTM version
sender.Send(new SharpOSC.OscMessage("/qtm", "qtmversion"));
// Ask for general camera system and 3d marker parameters (returned as xml)
sender.Send(new SharpOSC.OscMessage("/qtm", "getparameters", "General", "3d"));
// Start streaming of frames (just 3d and 6dof data)
sender.Send(new SharpOSC.OscMessage("/qtm", "StreamFrames", "AllFrames", "3d", "6d"));
// Pressing the escape key will stop streaming and disconnect from qtm
while (true)
{
if (Console.KeyAvailable)
{
if (Console.ReadKey(false).Key == ConsoleKey.Escape)
{
break;
}
}
}
// Stop streaming
sender.Send(new SharpOSC.OscMessage("/qtm", "StreamFrames", "Stop"));
// Disconnect from qtm
sender.Send(new SharpOSC.OscMessage("/qtm", "disconnect"));
}
/// <summary>
/// Connect to a NatNet server (e.g. Motive)
/// </summary>
private static void Connect()
{
// [NatNet] connect to a NatNet server
int returnCode = 0;
string strLocalIP = "127.0.0.1";
string strServerIP = "127.0.0.1";
returnCode = m_NatNet.Initialize(strLocalIP, strServerIP);
if (returnCode == 0)
{
Console.WriteLine("Initialization Succeeded.");
}
else
{
Console.WriteLine("Error Initializing.");
}
// [NatNet] validate the connection
returnCode = m_NatNet.GetServerDescription(desc);
if (returnCode == 0)
{
Console.WriteLine("Connection Succeeded.");
Console.WriteLine(" Server App Name: " + desc.HostApp);
Console.WriteLine(String.Format(" Server App Version: {0}.{1}.{2}.{3}", desc.HostAppVersion[0], desc.HostAppVersion[1], desc.HostAppVersion[2], desc.HostAppVersion[3]));
Console.WriteLine(String.Format(" Server NatNet Version: {0}.{1}.{2}.{3}", desc.NatNetVersion[0], desc.NatNetVersion[1], desc.NatNetVersion[2], desc.NatNetVersion[3]));
// Setup OSC sender to remote computer
sender = new SharpOSC.UDPSender(oscIP, Int32.Parse(oscPORT));
Console.WriteLine("set up OSC sender at " + oscIP + " on port " + oscPORT);
// Set up OSC
// Let the user set the IP Address and Port for sending out OSC info
Console.WriteLine("Enter IP Address of Remote Computer receiving data: ");
String customOscIP = Console.ReadLine();
Console.WriteLine("Enter PORT of Remote Computer receiving data: ");
String customOscPORT = Console.ReadLine();
// offer a use a default IP / PORT
if (customOscIP.Length == 0 && customOscPORT.Length == 0)
{
oscIP = "128.237.161.161";
oscPORT = "55555";
Console.WriteLine("...Using default IP and PORT: " + oscIP + ":" + oscPORT);
}
else
{
oscIP = customOscIP;
oscPORT = customOscPORT;
Console.WriteLine("...Setting custom IP and PORT: " + oscIP + ":" + oscPORT);
}
// Tracking Tools and Motive report in meters - lets convert to millimeters
if (desc.HostApp.Contains("TrackingTools") || desc.HostApp.Contains("Motive"))
m_ServerToMillimeters = 1000.0f;
// [NatNet] [optional] Query mocap server for the current camera framerate
int nBytes = 0;
byte[] response = new byte[10000];
int rc;
rc = m_NatNet.SendMessageAndWait("FrameRate", out response, out nBytes);
if (rc == 0)
{
try
{
m_ServerFramerate = BitConverter.ToSingle(response, 0);
Console.WriteLine(String.Format(" Camera Framerate: {0}", m_ServerFramerate));
}
catch (System.Exception ex)
{
Console.WriteLine(ex.Message);
}
}
// [NatNet] [optional] Query mocap server for the current analog framerate
rc = m_NatNet.SendMessageAndWait("AnalogSamplesPerMocapFrame", out response, out nBytes);
if (rc == 0)
{
try
{
mAnalogSamplesPerMocpaFrame = BitConverter.ToInt32(response, 0);
Console.WriteLine(String.Format(" Analog Samples Per Camera Frame: {0}", mAnalogSamplesPerMocpaFrame));
}
catch (System.Exception ex)
{
Console.WriteLine(ex.Message);
}
}
// [NatNet] [optional] Query mocap server for the current up axis
rc = m_NatNet.SendMessageAndWait("UpAxis", out response, out nBytes);
if (rc == 0)
{
m_UpAxis = BitConverter.ToInt32(response, 0);
}
m_fCurrentMocapFrameTimestamp = 0.0f;
m_fFirstMocapFrameTimestamp = 0.0f;
mDroppedFrames = 0;
}
else
{
Console.WriteLine("Error Connecting.");
}
}
/*
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]);
}
}
///Execute Startup
/// <param name="sender">object sending the event</param>
/// <param name="e">event arguments</param>
///
private void WindowLoaded(object sender, RoutedEventArgs e)
{
// Create the drawing group we'll use for drawing
this.drawingGroup = new DrawingGroup();
// Create an image source that we can use in our image control
this.imageSource = new DrawingImage(this.drawingGroup);
// Display the drawing using our image control
Image.Source = this.imageSource;
//initialize the pair and joint on flags to 'false'
//set up position and distance registers appropriately
for (int i=0; i < numP; i++)
{
pOn[i] = false;
pReg[i] = new distance_register();
}
for (int i = 0; i < numJ; i++)
{
jOn[i] = false;
jReg[i] = new position_Register();
}
//initializes IP addresses
IPs[0] = "127.0.0.1";
IPs[1] = enterIP2.Text;
IPs[2] = enterIP3.Text;
IPs[3] = enterIP4.Text;
IPs[4] = enterIP5.Text;
IPs[5] = enterIP6.Text;
//initializes ports
ports[0] = System.Convert.ToInt32(enterPort.Text, 10);
ports[1] = System.Convert.ToInt32(enterPort2.Text, 10);
ports[2] = System.Convert.ToInt32(enterPort3.Text, 10);
ports[3] = System.Convert.ToInt32(enterPort4.Text, 10);
ports[4] = System.Convert.ToInt32(enterPort5.Text, 10);
ports[5] = System.Convert.ToInt32(enterPort6.Text, 10);
//initializes UDP senders to each IP and port
for (int i = 0; i < numSenders; i++)
{
UDPs[i] = new UDPSender(IPs[i],ports[i]);
UDPs[i].Port = ports[i];
UDPs[i].Address = IPs[i];
}
//populates routing matrix
for (int i = 0; i < numJ; i++)
{
for (int j = 0; j < numIPs; j++)
{
j_IP[i, j] = true;
}
}
for (int i = 0; i < numP; i++)
{
for (int j = 0; j < numIPs; j++)
{
p_IP[i, j] = true;
}
}
j_values[0] = (JointType)j01.SelectedValue;
j_values[1] = (JointType)j02.SelectedValue;
j_values[2] = (JointType)j03.SelectedValue;
j_values[3] = (JointType)j04.SelectedValue;
j_values[4] = (JointType)j05.SelectedValue;
j_values[5] = (JointType)j06.SelectedValue;
j_values[6] = (JointType)j07.SelectedValue;
j_values[7] = (JointType)j08.SelectedValue;
j_values[8] = (JointType)j09.SelectedValue;
j_values[9] = (JointType)j10.SelectedValue;
p_values[0, 0] = (JointType)p01A.SelectedValue;
p_values[0, 1] = (JointType)p01B.SelectedValue;
p_values[1, 0] = (JointType)p02A.SelectedValue;
p_values[1, 1] = (JointType)p02B.SelectedValue;
p_values[2, 0] = (JointType)p03A.SelectedValue;
p_values[2, 1] = (JointType)p03B.SelectedValue;
// Look through all sensors and start the first connected one.
// This requires that a Kinect is connected at the time of app startup.
foreach (var potentialSensor in KinectSensor.KinectSensors)
{
if (potentialSensor.Status == KinectStatus.Connected)
{
this.sensor = potentialSensor;
break;
}
}
if (null != this.sensor)
{
// Turn on the skeleton stream to receive skeleton frames
this.sensor.SkeletonStream.Enable(smoothParameters);
// Add an event handler to be called whenever there is new color frame data
this.sensor.SkeletonFrameReady += this.SensorSkeletonFrameReady;
// Start the sensor!
try
{
this.sensor.Start();
}
catch (IOException)
{
this.sensor = null;
}
}
if (null == this.sensor)
{
this.statusBarText.Text = Properties.Resources.NoKinectReady;
}
}
/*
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);
}
}
}
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 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);
}
}
}
static void Main(string[] args)
{
// Program options
bool TransmitMulticast = false;
bool EnableHapticTest = false;
bool bReadCentroids = false;
List<String> HapticOnList = new List<String>();
int Counter = 1;
string HostName = "localhost:801"; // connect to Vicon system locally
// string HostName = "192.168.1.1:801";
if (args.Length > 0)
{
HostName = args[0];
}
// parsing all the haptic arguments
for (int j = 1; j < args.Length; ++j)
{
string HapticArg = "--enable_haptic_test";
if (String.Compare(args[j], HapticArg) == 0)
{
EnableHapticTest = true;
++j;
while (j < args.Length && String.Compare(args[j], 0, "--", 0, 2) != 0)
{
HapticOnList.Add(args[j]);
++j;
}
}
string CentroidsArg = "--centroids";
if (String.Compare(args[j], CentroidsArg) == 0)
{
bReadCentroids = true;
}
}
// Make a new client
ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client();
// Connect to a server
Console.Write("Connecting to {0} ...", HostName);
while (!MyClient.IsConnected().Connected)
{
// Direct connection
MyClient.Connect(HostName);
// Multicast connection
// MyClient.ConnectToMulticast( HostName, "224.0.0.0" );
System.Threading.Thread.Sleep(200);
Console.Write(".");
}
Console.WriteLine();
// Enable some different data types
MyClient.EnableSegmentData();
MyClient.EnableMarkerData();
MyClient.EnableUnlabeledMarkerData();
MyClient.EnableDeviceData();
if (bReadCentroids)
{
MyClient.EnableCentroidData();
}
Console.WriteLine("Segment Data Enabled: {0}", MyClient.IsSegmentDataEnabled().Enabled);
Console.WriteLine("Marker Data Enabled: {0}", MyClient.IsMarkerDataEnabled().Enabled);
Console.WriteLine("Unlabeled Marker Data Enabled: {0}", MyClient.IsUnlabeledMarkerDataEnabled().Enabled);
Console.WriteLine("Device Data Enabled: {0}", MyClient.IsDeviceDataEnabled().Enabled);
Console.WriteLine("Centroid Data Enabled: {0}", MyClient.IsCentroidDataEnabled().Enabled);
// Set the streaming mode
MyClient.SetStreamMode(ViconDataStreamSDK.DotNET.StreamMode.ClientPull);
// MyClient.SetStreamMode( ViconDataStreamSDK.DotNET.StreamMode.ClientPullPreFetch );
// MyClient.SetStreamMode( ViconDataStreamSDK.DotNET.StreamMode.ServerPush );
// Set the global up axis
MyClient.SetAxisMapping(ViconDataStreamSDK.DotNET.Direction.Forward,
ViconDataStreamSDK.DotNET.Direction.Left,
ViconDataStreamSDK.DotNET.Direction.Up); // Z-up
// MyClient.SetAxisMapping( ViconDataStreamSDK.DotNET.Direction.Forward,
// ViconDataStreamSDK.DotNET.Direction.Up,
// ViconDataStreamSDK.DotNET.Direction.Right ); // Y-up
Output_GetAxisMapping _Output_GetAxisMapping = MyClient.GetAxisMapping();
Console.WriteLine("Axis Mapping: X-{0} Y-{1} Z-{2}", Adapt(_Output_GetAxisMapping.XAxis),
Adapt(_Output_GetAxisMapping.YAxis),
Adapt(_Output_GetAxisMapping.ZAxis));
// Discover the version number
Output_GetVersion _Output_GetVersion = MyClient.GetVersion();
Console.WriteLine("Version: {0}.{1}.{2}", _Output_GetVersion.Major,
_Output_GetVersion.Minor,
_Output_GetVersion.Point);
if (TransmitMulticast)
{
MyClient.StartTransmittingMulticast("localhost", "224.0.0.0");
}
// Let the user set the IP Address and Port for sending out OSC info
Console.WriteLine("Enter IP Address of Remote Computer receiving data: ");
String oscIP = Console.ReadLine();
Console.WriteLine("Enter PORT of Remote Computer receiving data: ");
String oscPORT = Console.ReadLine();
// offer a use a default IP / PORT
if (oscIP.Length == 0 && oscPORT.Length == 0)
{
oscIP = "192.168.125.20";
oscPORT = "55555";
Console.WriteLine("...Set to default IP and PORT: "+oscIP+":"+oscPORT);
}
// Setup OSC sender to remote computer
var sender = new SharpOSC.UDPSender(oscIP, Int32.Parse(oscPORT));// "10.140.76.75", 55555);
Console.WriteLine();
Console.WriteLine("Ready to Stream!");
Console.WriteLine("+ + + + + + + + + + + + ");
// Stream Mocap Data
while (true)
{
++Counter;
// Console.KeyAvailable throws an exception if stdin is a pipe (e.g.
// with TrackerDssdkTests.py), so we use try..catch:
try {
if (Console.KeyAvailable){
break;
}
}
catch (InvalidOperationException){}
// Get a frame
//Console.Write("Waiting for new frame...");
while (MyClient.GetFrame().Result != ViconDataStreamSDK.DotNET.Result.Success){
System.Threading.Thread.Sleep(200);
//Console.Write(".");
}
// stream out data to remote location via OSC
sendRigidBodies(MyClient, sender);
sendLocalRotationQuaternion(MyClient, sender);
sendLabledMarkers(MyClient, sender);
sendUnlabeledMarkers(MyClient,sender);
}
// shut down and disconnect
if (TransmitMulticast){
MyClient.StopTransmittingMulticast();
}
// Disconnect and dispose
MyClient.Disconnect();
MyClient = null;
}
static void Main(string[] args)
{
// The IP address of the server (the PC on which this program is running)
string sHostIpAddress = "128.2.102.159";
// Standard port number
int nPort = 21;
Console.WriteLine("Opening IP Address: " + sHostIpAddress);
IPAddress ipAddress = IPAddress.Parse(sHostIpAddress); // Create the IP address
Console.WriteLine("Starting to listen on port: " + nPort);
TcpListener tcpListener = new TcpListener(ipAddress, nPort); // Create the tcp Listener
tcpListener.Start(); // Start listening
// Set up OSC
// Let the user set the IP Address and Port for sending out OSC info
Console.WriteLine("Enter IP Address of Remote Computer receiving data: ");
String oscIP = Console.ReadLine();
Console.WriteLine("Enter PORT of Remote Computer receiving data: ");
String oscPORT = Console.ReadLine();
// offer a use a default IP / PORT
if (oscIP.Length == 0 && oscPORT.Length == 0)
{
oscIP = "128.237.191.176";
oscPORT = "55555";
Console.WriteLine("...Set to default IP and PORT: " + oscIP + ":" + oscPORT);
}
// Setup OSC sender to remote computer
var sender = new SharpOSC.UDPSender(oscIP, Int32.Parse(oscPORT));// "10.140.76.75", 55555);
Console.WriteLine();
Console.WriteLine("Ready to Stream!");
Console.WriteLine("+ + + + + + + + + + + + ");
// Setup OSC listener
HandleOscPacket callback = delegate(OscPacket packet)
{
var oscMsg = (OscMessage)packet;
String addr = oscMsg.Address;
if (addr.Equals("/pose"))
OSCinput = "getTcpPose";
else if (addr.Equals("/target"))
OSCinput = "getTargetPose";
else if (addr.Equals("/joints"))
OSCinput = "getJointPose";
else if (addr.Equals("/force"))
OSCinput = "getForce";
else if (addr.Equals("/tcpForce"))
OSCinput = "getTcpForce";
else if (addr.Equals("/movej"))
{
OSCinput = "movej";
// print out the incoming osc message
//Console.WriteLine(addr);
for (int i = 0; i < oscMsg.Arguments.Count; i++)
{
pose[i] = float.Parse(oscMsg.Arguments[i].ToString());
// Console.WriteLine(" " + pose[i]);
}
//Console.WriteLine("");
}
};
var listener = new UDPListener(12000, callback);
// Keep on listening forever
while (true)
{
TcpClient tcpClient = tcpListener.AcceptTcpClient(); // Accept the client
Console.WriteLine("Accepted new client");
NetworkStream stream = tcpClient.GetStream(); // Open the network stream
while (tcpClient.Client.Connected)
{
// Console.Write("Send msg to Robot OVER OSC: ");
// string input = Console.ReadLine();
// if we've gotten an OSC message
if (!OSCinput.Equals(""))
{
sendMsg(stream, OSCinput);
string msg = "";
while (msg.Equals(""))
{
msg = receiveMsg(stream, tcpClient, sender);
}
// if we are sending an additional command
if (OSCinput.Equals("movej"))
{
string poseMsg = "(";
for (int i = 0; i < 6; i++)
{
poseMsg += pose[i];
if (i < 5)
poseMsg += ", ";
else
poseMsg += ")";
}
Console.WriteLine(poseMsg);
sendMsg(stream, poseMsg);
}
// reset the OSC input
OSCinput = "";
}
}
}
//listener.Close();
}
