00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012 #include "tracking.h"
00013
00014
00015 using namespace VRUT;
00016
00017 Tracking::Tracking(const VRUT::MODULE_ID &_id, const wxString &_name, VRUT::EventHandler *msgSink)
00018 : Module(_id, _name, 0, msgSink)
00019 {
00020 REGISTER_PARAM_GUI_TEXTCONTROL(bodyIDParamID, wxT("bodyID"), wxT("0"), wxT("ID of tracking body."));
00021 REGISTER_PARAM_GUI_TEXTCONTROL(bodyTypeParamID, wxT("bodyType"), wxT("0"), wxT("Type of tracking body."));
00022 REGISTER_PARAM_GUI_TEXTCONTROL(rotationFilterParamID, wxT("rotationFilter"), wxT("0.1"), wxT("Rotation filter limit in Radians."));
00023 REGISTER_PARAM_GUI_TEXTCONTROL(translationFilterParamID, wxT("translationFilter"), wxT("5.0"), wxT("Translation filter limit."));
00024 REGISTER_PARAM_GUI_BUTTON(updateTableParamID, wxT("updateTable"), wxT("Update filtering table."));
00025 REGISTER_PARAM_GUI_BUTTON(dumpTableParamID, wxT("dumpTable"), wxT("Dump filtering table."));
00026 REGISTER_PARAM_GUI_TEXTCONTROL(IDoffsetParamID, wxT("IDoffset"), wxT("0"), wxT("Offset for IDs for given tracking device."));
00027 REGISTER_PARAM_GUI_TEXTCONTROL(localPortParamID, wxT("localPort"), wxT("2627"), wxT("Local port used for listening."));
00028 REGISTER_PARAM_GUI_TEXTCONTROL(bufferSizeParamID, wxT("bufferSize"), wxT("8192"), wxT("Size of buffer for packet reading."));
00029 REGISTER_PARAM_GUI_TEXTCONTROL(packetTimeoutParamID, wxT("packetTimeout"), wxT("20"), wxT("Timeout for packet reading."));
00030 REGISTER_PARAM_GUI_CHECKBOX(forwardParamID, wxT("forward"), wxT("0"), wxT("Forward tracking data to another application. (Useful for filtering.)"));
00031 REGISTER_PARAM_GUI_TEXTCONTROL(forwardMachinesParamID, wxT("forwardMachines"), wxT("4000@localhost"), wxT("List of machines to forward tracking data in format: port@host."));
00032 IDoffset = 0;
00033 rotationFilter = (float)0.1;
00034 bodyType = bodyID = 0;
00035 translationFilter = 5;
00036 remoteAddress = wxT("localhost");
00037 remotePort = 2627;
00038 localPort = 2627;
00039 eventTimeout= packetTimeout = 20;
00040 forward = false;
00041 forwardMachines=wxT("4000@localhost");
00042 listener = new Receiver();
00043 parser.SetQueue(&bodyQueue);
00044 sendSock=NULL;
00045 }
00046
00047 void Tracking::processEvent(wxCommandEvent & evt)
00048 {
00049 Module::processEvent(evt);
00050 switch (evt.GetEventType())
00051 {
00052 case Event::EVT_PARAM_SET:
00053 {
00054 updateTable = dumpTable = 0;
00055 UPDATE_PARAM_FROM_EVENT_INT(localPortParamID, localPort, evt);
00056 UPDATE_PARAM_FROM_EVENT_INT(bufferSizeParamID, bufferSize, evt);
00057 UPDATE_PARAM_FROM_EVENT_UNSIGNED(packetTimeoutParamID, packetTimeout,evt);
00058 UPDATE_PARAM_FROM_EVENT_FLOAT(rotationFilterParamID, rotationFilter, evt);
00059 UPDATE_PARAM_FROM_EVENT_FLOAT(translationFilterParamID, translationFilter, evt);
00060 UPDATE_PARAM_FROM_EVENT_INT(bodyIDParamID, bodyID, evt);
00061 UPDATE_PARAM_FROM_EVENT_INT(bodyTypeParamID, bodyType,evt);
00062 UPDATE_PARAM_FROM_EVENT_INT(updateTableParamID, updateTable, evt);
00063 UPDATE_PARAM_FROM_EVENT_INT(dumpTableParamID, dumpTable, evt);
00064 UPDATE_PARAM_FROM_EVENT_INT(IDoffsetParamID, IDoffset, evt);
00065 UPDATE_PARAM_FROM_EVENT_BOOL(forwardParamID, forward, evt);
00066 UPDATE_PARAM_FROM_EVENT_STRING(forwardMachinesParamID, forwardMachines, evt);
00067 if (forward&&((IS_PARAM(evt, forwardParamID))||(IS_PARAM(evt, forwardMachinesParamID))))
00068 {
00069 long port;
00070 forwardMachines.ToLong(&port);
00071 wxString host(wxT("localhost"));
00072 int at=forwardMachines.Find('@');
00073 if (at!=wxNOT_FOUND)
00074 host=forwardMachines.substr(at+1);
00075 local.AnyAddress();
00076 local.Service(port+1);
00077 remote.Hostname(host);
00078 remote.Service(port);
00079 SAFE_DELETE(sendSock);
00080 sendSock = new wxDatagramSocket(local, wxSOCKET_NOWAIT);
00081 if (sendSock->Error())
00082 LOGERROR(wxT("<Tracking>forward sendSock error"));
00083 }
00084 listener->UpdateSocket(remoteAddress, remotePort, localPort);
00085 listener->ResizeBuffer(bufferSize);
00086 eventTimeout = packetTimeout;
00087 if (updateTable != 0)
00088 {
00089 if(filter.UpdateFilter((Body::BodyType)bodyType, bodyID, rotationFilter, translationFilter))
00090 {
00091 LOG(wxT("<Tracking> Filter updated."));
00092 }
00093 else
00094 {
00095 LOG(wxT("<Tracking> Selected entry were not found! Entry Pre-created: "));
00096
00097 }
00098 }
00099 else if (dumpTable != 0)
00100 {
00101 filter.WriteToLog();
00102
00103 }
00104 break;
00105 }
00106 }
00107 if (packetTimeout != eventTimeout) packetTimeout = eventTimeout;
00108 }
00109 void Tracking::run()
00110 {
00111 while (listener->Receive())
00112 {
00113 parser.SetPacket(listener->GetPacket());
00114 if (parser.Parse())
00115 {
00116 processQueue();
00117 }
00118 else
00119 {
00120 LOGWARNING(wxT("<Tracking> Parsing Failed"));
00121 safeEmptyQueue();
00122 }
00123 }
00124 }
00125 wxString Tracking::GetDesc() const
00126 {
00127 return wxT("DTrack datagram receiver and parser");
00128 }
00129 Tracking::~Tracking()
00130 {
00131 listener->~Receiver();
00132 }
00133 void Tracking::safeEmptyQueue()
00134 {
00135 while (!bodyQueue.empty())
00136 {
00137 Body * tempB = bodyQueue.front();
00138 bodyQueue.pop();
00139 delete tempB;
00140 }
00141 }
00142 void Tracking::processQueue()
00143 {
00144 wxString fw;
00145 if (forward)
00146 {
00147 fw=wxString::Format(wxT("fr %i\r\n"), (int)parser.FrameCounter());
00148 }
00149 while (!bodyQueue.empty())
00150 {
00151 Body * tempA = bodyQueue.front();
00152 bodyQueue.pop();
00153 if (filter.Filter(tempA))
00154 {
00155
00156 }
00157 else
00158 {
00159
00160 DeviceData * data = new DeviceData;
00161 data->id = tempA->GetID() + IDoffset;
00162 data->type = tempA->GetType();
00163 data->matrix = tempA->Matrix();
00164 data->buttons = tempA->ButtonsMask32();
00165 data->control1 = tempA->Control(0);
00166 data->control2 = tempA->Control(1);
00167
00168 if (data->control1 < -1 || data->control1 > 1) data->control1 = 0;
00169 if (data->control2 < -1 || data->control2 > 1) data->control2 = 0;
00170
00171 wxCommandEvent evt = Event::GET_EVT_INPUT_TRACKING(data);
00172 PostToKernel(evt);
00173 if (forward)
00174 {
00175 switch (data->type)
00176 {
00177 case Body::B6D:
00178 {
00179 float x, y, z;
00180 data->matrix.ToEuler(x, y, z);
00181 fw+=wxString::Format(wxT("6d 1 [%i 1.0][%g %g %g %g %g %g][%g %g %g %g %g %g %g %g %g] \r\n"), data->id,
00182 data->matrix._m41, data->matrix._m42, data->matrix._m43, x/3.14159*180, -y/3.14159*180, z/3.14159*180,
00183 data->matrix._m11, data->matrix._m21, data->matrix._m31, data->matrix._m12, data->matrix._m22, data->matrix._m32, data->matrix._m13, data->matrix._m23, data->matrix._m33);
00184 break;
00185 }
00186 case Body::B6DF:
00187 {
00188 if (tempA->GetQuality()==-1)
00189 fw+=wxString::Format(wxT("6df 1 [%i -1.0 %i][0 0 0 0 0 0][0 0 0 0 0 0 0 0 0] \r\n"), data->id, data->buttons);
00190 else
00191 {
00192 float x, y, z;
00193 data->matrix.ToEuler(x, y, z);
00194 fw+=wxString::Format(wxT("6df 1 [%i 1.0 %i][%g %g %g %g %g %g][%g %g %g %g %g %g %g %g %g] \r\n"), data->id, data->buttons,
00195 data->matrix._m41, data->matrix._m42, data->matrix._m43, x/3.14159*180, -y/3.14159*180, z/3.14159*180,
00196 data->matrix._m11, data->matrix._m21, data->matrix._m31, data->matrix._m12, data->matrix._m22, data->matrix._m32, data->matrix._m13, data->matrix._m23, data->matrix._m33);
00197 }
00198 break;
00199 }
00200 default:
00201 break;
00202 }
00203 }
00204 }
00205 delete tempA;
00206 }
00207 if (forward)
00208 {
00209 sendSock->SendTo(remote, fw.mb_str(wxConvISO8859_1), (wxUint32)fw.size());
00210 if (sendSock->Error())
00211 LOGERROR(wxString::Format(wxT("<Tracking> Forward send error %i"), sendSock->LastError()));
00212 }
00213 }
00214 void Tracking::dumpFilterTable()
00215 {
00216
00217 wxString dump = wxT("<Tracking> FilterTable:\nType\tID\tAngleFilter\tDistFilter\n");
00218 dump += filter.GetAsString("\n","\t");
00219 LOG(dump);
00220 }
