Vicon socket communication to MATLAB

Socket communicate vicon information to its server/client pc.

Protocol

AA/BB/CCCC/DDDD/EEEE

AA/BB/CCCC/DDDD/EEEE

AA: Subject index

BB: Marker Index

CCCC: X location in [mm]

DDDD: Y location in [mm]

EEEE: Z location in [mm]

C++ Code

Vicon2MATLAB.zip

 

MATLAB Code

main.m

ccc

address = java.net.InetAddress.getLocalHost;

IPaddress = char(address.getHostAddress);

fprintf('CURRENT IP IS [%s]. \n', IPaddress);

portnum = 30001;

t = tcpip('0.0.0.0', portnum, 'NetworkRole', 'server');

fprintf('STARTING SERVER ON PORT %d. \n', portnum);

fopen(t);

fprintf(2, 'CONNECTED! \n');

fwrite(t, 'CONNECTED');

flag = 1;

nrx = 0;

iclk = clock;

 

figure(1);

hold on;

 

while flag

    nbytes = t.BytesAvailable;

    if nbytes > 0

        % RECEIVE

        nrx = nrx + 1;

        cmsec = etime(clock, iclk)*1000;

        data = fread(t, nbytes);

        string = char(data');

        fps = nrx / cmsec * 1000;

        % fprintf('[%.1fms / %.1ffps] %d-bytes: \n%s\n', cmsec, fps, nbytes, string);

        

        % SEND BACK 'A'

        fwrite(t, 'A');

        % PARSE INPUT

        token = strsplit(string, '\n');

        ntoken = length(token)-1;

        pos1list = zeros(3, 3);

        pos2list = zeros(3, 3);

        for i = 1:ntoken

            strtemp = token{i};

            token2 = strsplit(strtemp, '/');

            sidx = str2num(token2{1}) + 1;

            midx = str2num(token2{2}) + 1;

            x = str2double(token2{3});

            y = str2double(token2{4});

            z = str2double(token2{5});

            switch sidx

                case 1

                    pos1list(midx, :) = [x y z];

                case 2

                    pos2list(midx, :) = [x y z];

            end

        end

        pos1 = mean(pos1list);

        pos2 = mean(pos2list);

        

        if isequal(pos1, [0 0 0]) || isequal(pos2, [0 0 0])

            continue;

        end

        

        % PLOT

        plot_vicon_demo(pos1, pos2);

        drawnow;

    end

end

fprintf(2, 'Done. \n');

plot_vicon_demo.m

function plot_vicon_demo(pos1, pos2, xyzmargin)

persistent first_flsg h1 h2

 

if isempty(first_flsg)

    first_flsg = true;

end

 

if first_flsg

    first_flsg = false;

    h1 = plot3(pos1(1), pos1(2), pos1(3), 'ro', 'MarkerSize', 20, 'LineWidth', 3);

    h2 = plot3(pos2(1), pos2(2), pos2(3), 'bo', 'MarkerSize', 20, 'LineWidth', 3);

    

    pos_avg = (pos1 + pos2)/2;

    xavg = pos_avg(1);

    yavg = pos_avg(2);

    zavg = pos_avg(3);

    

    axis equal;

    % axis([-1000 1000 - 2000 0 1600]);

    xm =  xyzmargin(1);

    ym =  xyzmargin(2);

    zm =  xyzmargin(3);

    axis([xavg-xm xavg+xm yavg-ym yavg+ym zavg-zm zavg+zm]);

    grid on;

    view(56, 44);

else

    h1.XData = pos1(1);

    h1.YData = pos1(2);

    h1.ZData = pos1(3);

    h2.XData = pos2(1);

    h2.YData = pos2(2);

    h2.ZData = pos2(3);

end