view | history | commit | commitdiff
Janis Streib
19.05.23 446c564ad4ac3d84da8a5d9d0d682c32ba1a4a6d
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);
446c56 89     // FIXME: Do something gracefully
JS 90     exit(1);
929277 91 }
JS 92
93 int main(int argc,char* argv[]){
94
95   CG4Trk trk;
96   REND_STRUCT rs;
e9b870 97   string cfgFile;
e43149 98   string oscSinkAddr;
JS 99   int oscSinkPort;
929277 100
e9b870 101   auto cli = (
d6f0ba 102     option("-t", "--text").set(use_text).doc("send strings in osc messages instead of a float bundle"),
e9b870 103         value("c4g configuration file", cfgFile),
e43149 104         value("osc sink address", oscSinkAddr),
c3b1e1 105         integer("osc sink port", oscSinkPort),
JS 106         integer("polling period in ms (imprecise). You realistically won't need less than 8 ms.", pollPeriod)
e9b870 107   );
929277 108
c3b1e1 109   if(!parse(argc, argv, cli) || pollPeriod <= 0) {
e9b870 110       cout << make_man_page(cli, argv[0]);
JS 111     return 1;
112   }
e43149 113   transmitSocket = std::make_unique<UdpTransmitSocket>( IpEndpointName( oscSinkAddr.c_str(), oscSinkPort ) );
929277 114   memset(rs.viewTrans,0,sizeof(float)*3);
JS 115   rs.srcScale=rs.senScale=0.5;
116   rs.counter=0;
117
e9b870 118   int resp=trk.Connect(const_cast<char*>(cfgFile.c_str())); // whaaaaa
929277 119   if (!resp){
JS 120     printf("Error Connecting to G4\n");
121     return -1;
122   }
123  
124   usleep(500000);
125   rs.numSrc=trk.GetNumSrc();
126   rs.srcList=new float[rs.numSrc][6];
127   trk.GetSrcLoc(rs.srcList);
128
129   rs.numHub=trk.UpdateHubs();
130   rs.hubList=new CG4Hub[rs.numHub];
131   int* hubIds=new int[rs.numHub];
132   trk.GetHubList(hubIds,rs.numHub);
133   for (int i=0;i<rs.numHub;i++){
134     rs.hubList[i].SetId(hubIds[i]);
135   }
136   delete[] hubIds;
137
138   rs.pTrk=&trk;
139  
140   RunCollect=1;
141
3dfc1c 142   // register signal handler
JS 143   struct sigaction sigIntHandler;
144
145   sigIntHandler.sa_handler = sighandler;
146   sigemptyset(&sigIntHandler.sa_mask);
147   sigIntHandler.sa_flags = 0;
148
149   sigaction(SIGINT, &sigIntHandler, NULL);
150
929277 151   Collect(&rs);
JS 152
153   delete[] rs.srcList;
154   delete[] rs.hubList;
155
156   return 0;
157 }
158