view | history | commit | commitdiff
Janis Streib
19.05.23 c3b1e1f0860b157fdd0e0fe54dd743687e4065ab
commit | author | age
929277 1 #include <stdio.h>
e9b870 2 #include <iostream>
929277 3 #include "Quaternion.h"
JS 4 #include "struct.h"
5 #include "G4Trk.h"
6 #include "G4Hub.h"
7 #include <string.h>
8 #include <unistd.h>
e43149 9 #include <memory>
e9b870 10 #include "clipp.h"
e43149 11 #include <ip/UdpSocket.h>
JS 12 #include <osc/OscOutboundPacketStream.h>
d6f0ba 13 #include <sstream>
3dfc1c 14 #include <signal.h>
d6f0ba 15
e9b870 16 using namespace clipp; using std::cout; using std::string;
929277 17
e43149 18 #define OUTPUT_BUFFER_SIZE 512
929277 19
JS 20 int RunCollect=0;
e43149 21 std::unique_ptr<UdpTransmitSocket> transmitSocket;
d6f0ba 22 bool use_text = false;
c3b1e1 23 int pollPeriod = 8;
929277 24
JS 25
26 void* Collect(REND_STRUCT* prs){
3dfc1c 27     int iter = 0;
929277 28     CG4Trk* pTrk = prs->pTrk;
JS 29     while (RunCollect){
30         // periodically check for new hubs coming on line
3dfc1c 31         if(iter % 1000 == 0) {
929277 32             int numHubs;
JS 33             if (prs->tot_hubs_on_system!=prs->numHub){
34                 prs->numHub=numHubs=prs->tot_hubs_on_system=pTrk->UpdateHubs();
35                 int colorInd=0;
36                 if (prs->hubList) {
37                     delete[] prs->hubList;
38                 }
39                 prs->hubList=new CG4Hub[numHubs];
40                 int* hubIds=new int[numHubs];
41                 pTrk->GetHubList(hubIds,numHubs);
42                 for (int i=0;i<numHubs;i++){
43                     prs->hubList[i].SetId(hubIds[i]);
44                 }
45                 delete[] hubIds;
46             }
47
48         }
3dfc1c 49         iter++;
929277 50         prs->hubreadmap=0;
JS 51         prs->tot_hubs_on_system=prs->pTrk->GetHubsPno(prs);
e43149 52         char buffer[OUTPUT_BUFFER_SIZE];
JS 53
929277 54         for (int i=0;i<prs->numHub;i++){
ee3cd1 55             osc::OutboundPacketStream p( buffer, OUTPUT_BUFFER_SIZE );
95f3f3 56             if(!use_text) {
JS 57                 p << osc::BeginBundleImmediate;
58             }
3dfc1c 59             auto dat = prs->hubList[i].GetPnoData();
d05d79 60             for(int j=0;j<G4_SENSORS_PER_HUB;j++) {
JS 61                 if(!prs->hubList[i].IsSenActive(j)){
62                     continue;
63                 }
a59e92 64                 p << osc::BeginMessage( (string("/g4/hub/") + std::to_string(prs->hubList[i].GetId()) + string("/") + std::to_string(j)).c_str());
d05d79 65                 std::ostringstream ss;
JS 66                 if(use_text) {
3dfc1c 67                     ss << "[" << dat[j][0] << "," << dat[j][1] << "," << dat[j][2] << "," << dat[j][3] << "," << dat[j][4] << "," << dat[j][5] << "]";
d05d79 68                     auto res = ss.str();
JS 69                     p << res.c_str();
70                 } else {
3dfc1c 71                     p << dat[j][0] << dat[j][1] << dat[j][2] << dat[j][3] << dat[j][4] << dat[j][5];
d05d79 72                 }
JS 73                 p << osc::EndMessage;
d6f0ba 74             }
d05d79 75             if(!use_text) {
JS 76                 p << osc::EndBundle;
77             }
78             transmitSocket->Send( p.Data(), p.Size() );
929277 79         }
c3b1e1 80         usleep(pollPeriod*1000);
929277 81         // usleep(8000);
JS 82     }
83
84     return NULL;
3dfc1c 85 }
JS 86
87 void sighandler(int s){
88     printf("Caught signal %d\n",s);
89     RunCollect = 0;
929277 90 }
JS 91
92 int main(int argc,char* argv[]){
93
94   CG4Trk trk;
95   REND_STRUCT rs;
e9b870 96   string cfgFile;
e43149 97   string oscSinkAddr;
JS 98   int oscSinkPort;
929277 99
e9b870 100   auto cli = (
d6f0ba 101     option("-t", "--text").set(use_text).doc("send strings in osc messages instead of a float bundle"),
e9b870 102         value("c4g configuration file", cfgFile),
e43149 103         value("osc sink address", oscSinkAddr),
c3b1e1 104         integer("osc sink port", oscSinkPort),
JS 105         integer("polling period in ms (imprecise). You realistically won't need less than 8 ms.", pollPeriod)
e9b870 106   );
929277 107
c3b1e1 108   if(!parse(argc, argv, cli) || pollPeriod <= 0) {
e9b870 109       cout << make_man_page(cli, argv[0]);
JS 110     return 1;
111   }
e43149 112   transmitSocket = std::make_unique<UdpTransmitSocket>( IpEndpointName( oscSinkAddr.c_str(), oscSinkPort ) );
929277 113   memset(rs.viewTrans,0,sizeof(float)*3);
JS 114   rs.srcScale=rs.senScale=0.5;
115   rs.counter=0;
116
e9b870 117   int resp=trk.Connect(const_cast<char*>(cfgFile.c_str())); // whaaaaa
929277 118   if (!resp){
JS 119     printf("Error Connecting to G4\n");
120     return -1;
121   }
122  
123   usleep(500000);
124   rs.numSrc=trk.GetNumSrc();
125   rs.srcList=new float[rs.numSrc][6];
126   trk.GetSrcLoc(rs.srcList);
127
128   rs.numHub=trk.UpdateHubs();
129   rs.hubList=new CG4Hub[rs.numHub];
130   int* hubIds=new int[rs.numHub];
131   trk.GetHubList(hubIds,rs.numHub);
132   for (int i=0;i<rs.numHub;i++){
133     rs.hubList[i].SetId(hubIds[i]);
134   }
135   delete[] hubIds;
136
137   rs.pTrk=&trk;
138  
139   RunCollect=1;
140
3dfc1c 141   // register signal handler
JS 142   struct sigaction sigIntHandler;
143
144   sigIntHandler.sa_handler = sighandler;
145   sigemptyset(&sigIntHandler.sa_mask);
146   sigIntHandler.sa_flags = 0;
147
148   sigaction(SIGINT, &sigIntHandler, NULL);
149
929277 150   Collect(&rs);
JS 151
152   delete[] rs.srcList;
153   delete[] rs.hubList;
154
155   return 0;
156 }
157