view | history | commit | commitdiff
Janis Streib
19.05.23 7fde24bbdc2208dca6c2436f62ee1cb5eb9eb7b0
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             }
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         }
c3b1e1 90         usleep(pollPeriod*1000);
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   }
e43149 123   transmitSocket = std::make_unique<UdpTransmitSocket>( IpEndpointName( oscSinkAddr.c_str(), oscSinkPort ) );
929277 124   memset(rs.viewTrans,0,sizeof(float)*3);
JS 125   rs.srcScale=rs.senScale=0.5;
126   rs.counter=0;
127
e9b870 128   int resp=trk.Connect(const_cast<char*>(cfgFile.c_str())); // whaaaaa
929277 129   if (!resp){
JS 130     printf("Error Connecting to G4\n");
131     return -1;
132   }
133  
134   usleep(500000);
135   rs.numSrc=trk.GetNumSrc();
136   rs.srcList=new float[rs.numSrc][6];
137   trk.GetSrcLoc(rs.srcList);
138
139   rs.numHub=trk.UpdateHubs();
140   rs.hubList=new CG4Hub[rs.numHub];
141   int* hubIds=new int[rs.numHub];
142   trk.GetHubList(hubIds,rs.numHub);
143   for (int i=0;i<rs.numHub;i++){
144     rs.hubList[i].SetId(hubIds[i]);
145   }
146   delete[] hubIds;
147
148   rs.pTrk=&trk;
149  
150   RunCollect=1;
151
3dfc1c 152   // register signal handler
JS 153   struct sigaction sigIntHandler;
154
155   sigIntHandler.sa_handler = sighandler;
156   sigemptyset(&sigIntHandler.sa_mask);
157   sigIntHandler.sa_flags = 0;
158
159   sigaction(SIGINT, &sigIntHandler, NULL);
160
929277 161   Collect(&rs);
JS 162
163   return 0;
164 }
165