#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>
|
#include <signal.h>
|
#include <chrono>
|
#include <thread>
|
|
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;
|
int pollPeriod;
|
|
|
void* Collect(REND_STRUCT* prs){
|
int iter = 0;
|
CG4Trk* pTrk = prs->pTrk;
|
while (RunCollect){
|
// periodically check for new hubs coming on line
|
if(iter % 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;
|
}
|
|
}
|
iter++;
|
prs->hubreadmap=0;
|
prs->tot_hubs_on_system=prs->pTrk->GetHubsPno(prs);
|
char buffer[OUTPUT_BUFFER_SIZE];
|
|
for (int i=0;i<prs->numHub;i++){
|
osc::OutboundPacketStream p( buffer, OUTPUT_BUFFER_SIZE );
|
if(!use_text) {
|
p << osc::BeginBundleImmediate;
|
}
|
p << osc::BeginMessage( (string("/g4/hub/") + std::to_string(prs->hubList[i].GetId()) + string("/digio")).c_str());
|
std::ostringstream ss;
|
if(use_text) {
|
ss << prs->hubList[i].GetDigIOData();
|
auto res = ss.str();
|
p << res.c_str();
|
} else {
|
p << osc::int64(prs->hubList[i].GetDigIOData());
|
}
|
p << osc::EndMessage;
|
auto dat = prs->hubList[i].GetPnoData();
|
for(int j=0;j<G4_SENSORS_PER_HUB;j++) {
|
if(!prs->hubList[i].IsSenActive(j)){
|
continue;
|
}
|
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[j][0] << "," << dat[j][1] << "," << dat[j][2] << "," << dat[j][3] << "," << dat[j][4] << "," << dat[j][5] << "]";
|
auto res = ss.str();
|
p << res.c_str();
|
} else {
|
p << dat[j][0] << dat[j][1] << dat[j][2] << dat[j][3] << dat[j][4] << dat[j][5];
|
}
|
p << osc::EndMessage;
|
}
|
if(!use_text) {
|
p << osc::EndBundle;
|
}
|
transmitSocket->Send( p.Data(), p.Size() );
|
}
|
std::this_thread::sleep_for(std::chrono::milliseconds(pollPeriod));
|
}
|
|
g4_close_tracker();
|
return NULL;
|
}
|
|
void sighandler(int s){
|
printf("Caught signal %d\n",s);
|
RunCollect=0;
|
}
|
|
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),
|
integer("polling period in ms (imprecise). You realistically won't need less than 8 ms.", pollPeriod)
|
);
|
|
if(!parse(argc, argv, cli) || pollPeriod <= 0) {
|
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;
|
|
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;
|
|
// register signal handler
|
struct sigaction sigIntHandler;
|
|
sigIntHandler.sa_handler = sighandler;
|
sigemptyset(&sigIntHandler.sa_mask);
|
sigIntHandler.sa_flags = 0;
|
|
sigaction(SIGINT, &sigIntHandler, NULL);
|
|
Collect(&rs);
|
|
return 0;
|
}
|
|
