view | history | commit | commitdiff
Janis Streib
19.05.23 18a72e264ba3c6e3684f68b7745fc64a7673a7f0
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;
18a72e 23 int pollPeriod;
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             }
7fde24 59             p << osc::BeginMessage( (string("/g4/hub/") + std::to_string(prs->hubList[i].GetId()) + string("/digio")).c_str());
JS 60             std::ostringstream ss;
61             if(use_text) {
62                 ss << prs->hubList[i].GetDigIOData();
63                 auto res = ss.str();
64                 p << res.c_str();
65             } else {
66                 p << osc::int64(prs->hubList[i].GetDigIOData());
67             }
68             p << osc::EndMessage;
3dfc1c 69             auto dat = prs->hubList[i].GetPnoData();
d05d79 70             for(int j=0;j<G4_SENSORS_PER_HUB;j++) {
JS 71                 if(!prs->hubList[i].IsSenActive(j)){
72                     continue;
73                 }
a59e92 74                 p << osc::BeginMessage( (string("/g4/hub/") + std::to_string(prs->hubList[i].GetId()) + string("/") + std::to_string(j)).c_str());
d05d79 75                 std::ostringstream ss;
JS 76                 if(use_text) {
3dfc1c 77                     ss << "[" << dat[j][0] << "," << dat[j][1] << "," << dat[j][2] << "," << dat[j][3] << "," << dat[j][4] << "," << dat[j][5] << "]";
d05d79 78                     auto res = ss.str();
JS 79                     p << res.c_str();
80                 } else {
3dfc1c 81                     p << dat[j][0] << dat[j][1] << dat[j][2] << dat[j][3] << dat[j][4] << dat[j][5];
d05d79 82                 }
JS 83                 p << osc::EndMessage;
d6f0ba 84             }
d05d79 85             if(!use_text) {
JS 86                 p << osc::EndBundle;
87             }
88             transmitSocket->Send( p.Data(), p.Size() );
929277 89         }
18a72e 90         usleep(pollPeriod);
929277 91         // usleep(8000);
JS 92     }
93
69b11a 94     g4_close_tracker();
929277 95     return NULL;
3dfc1c 96 }
JS 97
98 void sighandler(int s){
99     printf("Caught signal %d\n",s);
69b11a 100     RunCollect=0;
929277 101 }
JS 102
103 int main(int argc,char* argv[]){
104
105   CG4Trk trk;
106   REND_STRUCT rs;
e9b870 107   string cfgFile;
e43149 108   string oscSinkAddr;
JS 109   int oscSinkPort;
929277 110
e9b870 111   auto cli = (
d6f0ba 112     option("-t", "--text").set(use_text).doc("send strings in osc messages instead of a float bundle"),
e9b870 113         value("c4g configuration file", cfgFile),
e43149 114         value("osc sink address", oscSinkAddr),
c3b1e1 115         integer("osc sink port", oscSinkPort),
JS 116         integer("polling period in ms (imprecise). You realistically won't need less than 8 ms.", pollPeriod)
e9b870 117   );
929277 118
c3b1e1 119   if(!parse(argc, argv, cli) || pollPeriod <= 0) {
e9b870 120       cout << make_man_page(cli, argv[0]);
JS 121     return 1;
122   }
18a72e 123   pollPeriod = pollPeriod*1000;
e43149 124   transmitSocket = std::make_unique<UdpTransmitSocket>( IpEndpointName( oscSinkAddr.c_str(), oscSinkPort ) );
929277 125   memset(rs.viewTrans,0,sizeof(float)*3);
JS 126   rs.srcScale=rs.senScale=0.5;
127   rs.counter=0;
128
e9b870 129   int resp=trk.Connect(const_cast<char*>(cfgFile.c_str())); // whaaaaa
929277 130   if (!resp){
JS 131     printf("Error Connecting to G4\n");
132     return -1;
133   }
134  
135   usleep(500000);
136   rs.numSrc=trk.GetNumSrc();
137   rs.srcList=new float[rs.numSrc][6];
138   trk.GetSrcLoc(rs.srcList);
139
140   rs.numHub=trk.UpdateHubs();
141   rs.hubList=new CG4Hub[rs.numHub];
142   int* hubIds=new int[rs.numHub];
143   trk.GetHubList(hubIds,rs.numHub);
144   for (int i=0;i<rs.numHub;i++){
145     rs.hubList[i].SetId(hubIds[i]);
146   }
147   delete[] hubIds;
148
149   rs.pTrk=&trk;
150  
151   RunCollect=1;
152
3dfc1c 153   // register signal handler
JS 154   struct sigaction sigIntHandler;
155
156   sigIntHandler.sa_handler = sighandler;
157   sigemptyset(&sigIntHandler.sa_mask);
158   sigIntHandler.sa_flags = 0;
159
160   sigaction(SIGINT, &sigIntHandler, NULL);
161
929277 162   Collect(&rs);
JS 163
164   return 0;
165 }
166