New reged

src/etc/viconmarkerbroadcaster/MarkerStream.cs

@@ -0,0 +1,166 @@
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+using System.Threading;
+using System.Net;
+using System.Net.Sockets;
+using System.Diagnostics;
+using ViconDataStreamSDK.DotNET;
+
+namespace ViconMarkerBroadcaster
+{
+    /// <remarks>
+    /// This class contains minimal functionality, used only to stream labeled 
+    /// and unlabeled marker data from Vicon over UDP to another program on 
+    /// the local computer. This was made to work with the LTLMoP map creator.
+    /// 
+    /// All input arguments are optional.
+    /// The port for UDP communication can be specified as the first argument 
+    /// when calling this program. Otherwise it will default to port 7500.
+    /// The frequency for UDP communication can be specified as the second 
+    /// argument when calling the program. This should match up with the update
+    /// frequency of the ViconMarkerListener in the LTLMoP map creator. If this
+    /// is not specified, it will default to 20 Hz.
+    /// 
+    /// Most of the code is copied and adjusted from DEASL.Components.MapEditor 
+    /// and DEASL.Resources.ViconBroadcaster.
+    /// </remarks>
+    class MarkerStream
+    {
+        static void Main(string[] args)
+        {
+            // Default update frequency for sending data to LTLMoP (Hz)
+            double DEFAULT_FREQ = 20.0;
+
+            // Default port used for communicating with the LTLMoP map creator.
+            int DEFAULT_PORT = 7500;
+
+            // Delay before declaring timeout if no data (ms)
+            int timeoutms = 5000;
+
+            // Process arguments
+            int port = DEFAULT_PORT;
+            double updateFreq = DEFAULT_FREQ;
+            if (args.Length > 0)
+            {
+                // Get port number
+                try
+                {
+                    port = Int32.Parse(args[0]);
+                }
+                catch (Exception)
+                {
+                    Console.WriteLine("Invalid argument for port number.");
+                    Console.WriteLine("Using default port: " + DEFAULT_PORT);
+                }
+
+                // Get update frequency
+                if (args.Length > 1)
+                {
+                    try
+                    {
+                        updateFreq = Double.Parse(args[1]);
+                    }
+                    catch (Exception)
+                    {
+                        Console.WriteLine("Invalid argument for update frequency.");
+                        Console.WriteLine("Using default frequency: " + DEFAULT_FREQ);
+                    }
+                }
+            }
+            int delayms = (int)(1000.0 / updateFreq);
+
+            // Stopwatch for checking timeout
+            Stopwatch swatch = new Stopwatch();
+
+            // Connect to Vicon
+            Client vicon = new Client();    // SDK class that controls communication with Vicon
+            Output_Connect res = vicon.Connect("10.0.0.102");
+            if (res.Result != Result.Success)
+            {
+                Console.WriteLine("Error while connecting to Vicon.");
+                Console.ReadKey();
+                vicon = null;
+                return;
+            }
+
+            // Set stream mode to "polling" of catched Vicon frames
+            if (vicon.SetStreamMode(StreamMode.ClientPullPreFetch).Result != Result.Success)
+            {
+                Console.WriteLine("Error while setting stream mode.");
+                Console.ReadKey();
+                return;
+            }
+
+            // Use unlabeled as well as labeled markers
+            // TODO: Make sure that labeled markers are read by default
+            if (vicon.EnableUnlabeledMarkerData().Result != Result.Success)
+            {
+                Console.WriteLine("Error while enabling unlabeled markers.");
+                Console.WriteLine("Press any key to exit");
+                Console.ReadKey();
+                return;
+            }
+
+            Console.WriteLine("Connecting to Vicon.");
+
+            // Set up port for connecting to LTLMoP
+            UdpClient controlClient = new UdpClient();
+            IPEndPoint destAddr = new IPEndPoint(IPAddress.Loopback, port);
+
+            Console.WriteLine("Requesting broadcasting to begin.");
+
+            // Update marker information and send over UDP until closed
+            swatch.Start();
+            while (true)
+            {
+                // Update vicon markers
+                Result frameResult = vicon.GetFrame().Result;
+                uint nMarkers = vicon.GetUnlabeledMarkerCount().MarkerCount;
+                if (frameResult == Result.NoFrame || (swatch.IsRunning && nMarkers == 0))
+                {
+                    if (swatch.IsRunning && swatch.ElapsedMilliseconds > timeoutms)
+                    {
+                        Console.WriteLine("No data received from Vicon for several seconds.");
+                        Console.WriteLine("Please check that Vicon is on and Vicon Nexus is started.");
+                        Console.WriteLine("Press any key to exit");
+                        Console.ReadKey();
+                        return;
+                    }
+                    continue;
+                }
+                else if (frameResult != Result.Success)
+                {
+                    Console.WriteLine("Error while enabling retrieving frame.");
+                    continue;
+                }
+
+                // Have received data so stop stopwatch
+                if (swatch.IsRunning)
+                {
+                    Console.WriteLine("Connection to Vicon confirmed. Data broadcasting.");
+                    swatch.Stop();
+                }
+                // Loop through frame, retrieving marker positions and adding 
+                // them to the byte-message to be sent over UDP
+                byte[] message = new byte[nMarkers * 16]; // 8 bytes, per x and y coordinate, per marker
+                for (uint i = 0; i < nMarkers; i++)
+                {
+                    double[] pos = vicon.GetUnlabeledMarkerGlobalTranslation(i).Translation;
+                    // Convert millimeters into meters, round to nearest millimeter, then convert to bytes
+                    byte[] x = BitConverter.GetBytes(Math.Round(pos[0] / 1000, 3));
+                    byte[] y = BitConverter.GetBytes(Math.Round(pos[1] / 1000, 3));
+                    System.Buffer.BlockCopy(x, 0, message, (int)i * 16, 8);
+                    System.Buffer.BlockCopy(y, 0, message, (int)i * 16 + 8, 8);
+                }
+
+                // Send message over UDP
+                controlClient.Send(message, message.Length, destAddr);
+
+                // Sleep until next update
+                Thread.Sleep(delayms);
+            }
+        }
+    }
+}

src/etc/viconmarkerbroadcaster/Properties/AssemblyInfo.cs

@@ -0,0 +1,36 @@
+using System.Reflection;
+using System.Runtime.CompilerServices;
+using System.Runtime.InteropServices;
+
+// General Information about an assembly is controlled through the following 
+// set of attributes. Change these attribute values to modify the information
+// associated with an assembly.
+[assembly: AssemblyTitle("ViconMarkerBroadcaster")]
+[assembly: AssemblyDescription("")]
+[assembly: AssemblyConfiguration("")]
+[assembly: AssemblyCompany("")]
+[assembly: AssemblyProduct("ViconMarkerBroadcaster")]
+[assembly: AssemblyCopyright("Copyright ©  2012")]
+[assembly: AssemblyTrademark("")]
+[assembly: AssemblyCulture("")]
+
+// Setting ComVisible to false makes the types in this assembly not visible 
+// to COM components.  If you need to access a type in this assembly from 
+// COM, set the ComVisible attribute to true on that type.
+[assembly: ComVisible(false)]
+
+// The following GUID is for the ID of the typelib if this project is exposed to COM
+[assembly: Guid("134a2e7d-e030-437b-8afb-c2a3d3f61bea")]
+
+// Version information for an assembly consists of the following four values:
+//
+//      Major Version
+//      Minor Version 
+//      Build Number
+//      Revision
+//
+// You can specify all the values or you can default the Build and Revision Numbers 
+// by using the '*' as shown below:
+// [assembly: AssemblyVersion("1.0.*")]
+[assembly: AssemblyVersion("1.0.0.0")]
+[assembly: AssemblyFileVersion("1.0.0.0")]

src/etc/viconmarkerbroadcaster/ViconMarkerBroadcaster.csproj

@@ -0,0 +1,64 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Project ToolsVersion="4.0" DefaultTargets="Build" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
+  <PropertyGroup>
+    <Configuration Condition=" '$(Configuration)' == '' ">Debug</Configuration>
+    <Platform Condition=" '$(Platform)' == '' ">x86</Platform>
+    <ProductVersion>8.0.30703</ProductVersion>
+    <SchemaVersion>2.0</SchemaVersion>
+    <ProjectGuid>{EB34323F-7ADB-45D9-97B5-BAFD12B78F84}</ProjectGuid>
+    <OutputType>Exe</OutputType>
+    <AppDesignerFolder>Properties</AppDesignerFolder>
+    <RootNamespace>ViconMarkerBroadcaster</RootNamespace>
+    <AssemblyName>ViconMarkerBroadcaster</AssemblyName>
+    <TargetFrameworkVersion>v4.0</TargetFrameworkVersion>
+    <TargetFrameworkProfile>Client</TargetFrameworkProfile>
+    <FileAlignment>512</FileAlignment>
+  </PropertyGroup>
+  <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|x86' ">
+    <PlatformTarget>x86</PlatformTarget>
+    <DebugSymbols>true</DebugSymbols>
+    <DebugType>full</DebugType>
+    <Optimize>false</Optimize>
+    <OutputPath>bin\Debug\</OutputPath>
+    <DefineConstants>DEBUG;TRACE</DefineConstants>
+    <ErrorReport>prompt</ErrorReport>
+    <WarningLevel>4</WarningLevel>
+  </PropertyGroup>
+  <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|x86' ">
+    <PlatformTarget>x86</PlatformTarget>
+    <DebugType>pdbonly</DebugType>
+    <Optimize>true</Optimize>
+    <OutputPath>bin\Release\</OutputPath>
+    <DefineConstants>TRACE</DefineConstants>
+    <ErrorReport>prompt</ErrorReport>
+    <WarningLevel>4</WarningLevel>
+  </PropertyGroup>
+  <ItemGroup>
+    <Reference Include="System" />
+    <Reference Include="System.Core" />
+    <Reference Include="System.Xml.Linq" />
+    <Reference Include="System.Data.DataSetExtensions" />
+    <Reference Include="Microsoft.CSharp" />
+    <Reference Include="System.Data" />
+    <Reference Include="System.Xml" />
+    <Reference Include="ViconDataStreamSDK_DotNET, Version=0.0.0.0, Culture=neutral, processorArchitecture=x86">
+      <SpecificVersion>False</SpecificVersion>
+      <HintPath>.\ViconDataStreamSDK_DotNET.dll</HintPath>
+    </Reference>
+  </ItemGroup>
+  <ItemGroup>
+    <Compile Include="MarkerStream.cs" />
+    <Compile Include="Properties\AssemblyInfo.cs" />
+  </ItemGroup>
+  <ItemGroup>
+    <None Include="app.config" />
+  </ItemGroup>
+  <Import Project="$(MSBuildToolsPath)\Microsoft.CSharp.targets" />
+  <!-- To modify your build process, add your task inside one of the targets below and uncomment it. 
+       Other similar extension points exist, see Microsoft.Common.targets.
+  <Target Name="BeforeBuild">
+  </Target>
+  <Target Name="AfterBuild">
+  </Target>
+  -->
+</Project>

src/etc/viconmarkerbroadcaster/ViconMarkerListener.py

@@ -0,0 +1,82 @@
+import time
+import struct
+import threading
+import socket
+
+class ViconMarkerListener(threading.Thread):
+    def __init__(self, freq=20.0, ip="0.0.0.0", port=7500, parent=None):
+        """Create the a socket to receive Vicon data.
+
+        freq - Update frequency in Hz.
+        ip - IP address to listen on.
+             Default is local computer.
+        port - Port to listen on.
+               Default matches ViconMarkerBroadcaster (C#) default
+        parent - Object that may be useful. Can change ProcessData method to
+                 make it do something with this object when data is received.
+        """
+        super(ViconMarkerListener, self).__init__()
+
+        # Communication parameters
+        self.updateFreq = freq
+        self.addr = (ip, port)
+        self.bufsize = 65536
+        self.udpSock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+        self.lock = threading.Lock()
+        self.close = threading.Event()
+
+        # Container for marker positions
+        self.poses = []         # List of tuples [(x1, y1), (x2, y2), ...]
+
+        # Creator of this object
+        self.parent = parent
+
+    def run(self):
+        """Open the socket to start communication. Process messages."""
+        # Open socket for communication
+        self.udpSock.bind(self.addr)
+
+        # Receive communication until stopped
+        self.close.clear()
+        delay = 1.0 / self.updateFreq
+        while not self.close.isSet():
+            self.lock.acquire()
+            data = self.udpSock.recv(self.bufsize)
+            self.lock.release()
+            self.ProcessData(data)
+            time.sleep(delay)
+
+        # Close socket
+        self.udpSock.close()
+
+    def stop(self):
+        """Close the socket to end UDP communication."""
+        self.close.set()
+
+    # Deserialize and save data
+    def ProcessData(self, data):
+        """Extract marker positions and keep them.
+
+        data - Byte array encoded from multiple pairs of doubles [x1 y1 ...]
+        """
+        # Check for valid data (not null or incomplete)
+        if data and len(data)%16 == 0:
+            self.poses = []
+            for i in range(0, len(data), 16):
+                x, y = struct.unpack('dd', data[i:i+16])
+                self.poses.append((x, y))
+            # If you want something to happen every time poses are recieved
+            # put that in here. You may need to pass in parameter parent.
+
+# Running by itself will just print out first 10 markers every second forever
+if __name__ == "__main__":
+    vicon = ViconMarkerListener()
+    vicon.start()
+    j = 0
+    while (1):
+        time.sleep(1)
+        print "Vicon Markers:"
+        for i in range(min(len(vicon.poses), 10)):
+            print "(%.3f, %.3f)" % vicon.poses[i]
+        print ""
+    vicon.stop()

src/etc/viconmarkerbroadcaster/app.config

@@ -0,0 +1,8 @@
+<?xml version="1.0"?>
+<configuration>
+<startup><supportedRuntime version="v4.0" sku=".NETFramework,Version=v4.0,Profile=Client"/></startup>
+
+  <startup useLegacyV2RuntimeActivationPolicy="true">
+    <supportedRuntime version="v4.0"/>
+  </startup>
+</configuration>

src/etc/viconmarkerbroadcaster/readme.txt

@@ -0,0 +1,18 @@
+Steps:
+1. Open ViconMarkerBroadcaster.csproj in Visual Studio.
+2. Save solution.
+3. Build solution as Release.
+4. Put these 4 files wherever you want, but they must be in the same directory:
+   (first three files are in /bin/Release/, while the other doesn't get transferred from the main folder)
+    ViconMarkerBroadcaster.exe
+    ViconMarkerBroadcaster.exe.config
+    ViconDataStreamSDK_DotNET.dll
+    ViconDataStreamSDK_CPP.dll
+5. Make sure Vicon is on.
+6. Run executable (recommend not running through Visual Studio to speed up the program).
+
+Alternative:
+4. Move ViconDataStreamSDK_CPP.dll to /bin/Release/
+5. Make a shortcut to ViconMarkerBroadcaster.exe and save it to a more accessible location.
+6. Make sure Vicon is on.
+7. Run executable through shortcut.