#include <stdio.h>
|
#include <iostream>
|
#include "Quaternion.h"
|
#include "struct.h"
|
#include "G4Trk.h"
|
#include "G4Hub.h"
|
#include <string.h>
|
#include <unistd.h>
|
#include <memory>
|
#include "clipp.h"
|
#include <ip/UdpSocket.h>
|
#include <osc/OscOutboundPacketStream.h>
|
#include <sstream>
|
|
using namespace clipp; using std::cout; using std::string;
|
|
#define OUTPUT_BUFFER_SIZE 512
|
|
int RunCollect=0;
|
std::unique_ptr<UdpTransmitSocket> transmitSocket;
|
bool use_text = false;
|
|
|
void* Collect(REND_STRUCT* prs){
|
int i = 0;
|
CG4Trk* pTrk = prs->pTrk;
|
while (RunCollect){
|
// periodically check for new hubs coming on line
|
if(i % 1000 == 0) {
|
int numHubs;
|
if (prs->tot_hubs_on_system!=prs->numHub){
|
prs->numHub=numHubs=prs->tot_hubs_on_system=pTrk->UpdateHubs();
|
int colorInd=0;
|
if (prs->hubList) {
|
delete[] prs->hubList;
|
}
|
prs->hubList=new CG4Hub[numHubs];
|
int* hubIds=new int[numHubs];
|
pTrk->GetHubList(hubIds,numHubs);
|
for (int i=0;i<numHubs;i++){
|
prs->hubList[i].SetId(hubIds[i]);
|
}
|
delete[] hubIds;
|
}
|
|
}
|
i++;
|
prs->hubreadmap=0;
|
prs->tot_hubs_on_system=prs->pTrk->GetHubsPno(prs);
|
char buffer[OUTPUT_BUFFER_SIZE];
|
osc::OutboundPacketStream p( buffer, OUTPUT_BUFFER_SIZE );
|
|
if(!use_text) {
|
p << osc::BeginBundleImmediate;
|
}
|
for (int i=0;i<prs->numHub;i++){
|
for(int j=0;j<G4_SENSORS_PER_HUB;j++) {
|
if(!prs->hubList[i].IsSenActive(j)){
|
continue;
|
}
|
auto dat = prs->hubList[i].GetPnoData()[j];
|
//printf("sns: %i: [%f;%f;%f],[%f,%f,%f]\n",i, dat[0],dat[1],dat[2], dat[3],dat[4],dat[5]);
|
p << osc::BeginMessage( (string("/g4/hub/") + std::to_string(prs->hubList[i].GetId()) + string("/") + std::to_string(j)).c_str());
|
std::ostringstream ss;
|
if(use_text) {
|
ss << "[" << dat[0] << "," << dat[1] << "," << dat[2] << "," << dat[3] << "," << dat[4] << "," << dat[5] << "]";
|
auto res = ss.str();
|
p << res.c_str();
|
} else {
|
p << dat[0] << dat[1] << dat[2] << dat[3] << dat[4] << dat[5];
|
}
|
p << osc::EndMessage;
|
}
|
if(!use_text) {
|
p << osc::EndBundle;
|
}
|
transmitSocket->Send( p.Data(), p.Size() );
|
}
|
usleep(15000);
|
// usleep(8000);
|
}
|
|
return NULL;
|
}
|
|
int main(int argc,char* argv[]){
|
|
CG4Trk trk;
|
REND_STRUCT rs;
|
string cfgFile;
|
string oscSinkAddr;
|
int oscSinkPort;
|
|
auto cli = (
|
option("-t", "--text").set(use_text).doc("send strings in osc messages instead of a float bundle"),
|
value("c4g configuration file", cfgFile),
|
value("osc sink address", oscSinkAddr),
|
integer("osc sink port", oscSinkPort)
|
);
|
|
if(!parse(argc, argv, cli)) {
|
cout << make_man_page(cli, argv[0]);
|
return 1;
|
}
|
transmitSocket = std::make_unique<UdpTransmitSocket>( IpEndpointName( oscSinkAddr.c_str(), oscSinkPort ) );
|
memset(rs.viewTrans,0,sizeof(float)*3);
|
rs.srcScale=rs.senScale=0.5;
|
rs.counter=0;
|
|
cfgFile="config.g4c";
|
int resp=trk.Connect(const_cast<char*>(cfgFile.c_str())); // whaaaaa
|
if (!resp){
|
printf("Error Connecting to G4\n");
|
return -1;
|
}
|
|
usleep(500000);
|
rs.numSrc=trk.GetNumSrc();
|
rs.srcList=new float[rs.numSrc][6];
|
trk.GetSrcLoc(rs.srcList);
|
|
rs.numHub=trk.UpdateHubs();
|
rs.hubList=new CG4Hub[rs.numHub];
|
int* hubIds=new int[rs.numHub];
|
trk.GetHubList(hubIds,rs.numHub);
|
for (int i=0;i<rs.numHub;i++){
|
rs.hubList[i].SetId(hubIds[i]);
|
}
|
delete[] hubIds;
|
|
rs.pTrk=&trk;
|
|
RunCollect=1;
|
|
Collect(&rs);
|
|
delete[] rs.srcList;
|
delete[] rs.hubList;
|
|
return 0;
|
}
|
|
