view | history | commit | commitdiff
Janis Streib
19.05.23 b21ff09151ca7e87348f1ea38944decc1f46f5db
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>
b21ff0 15 #include <chrono>
JS 16 #include <thread>
d6f0ba 17
e9b870 18 using namespace clipp; using std::cout; using std::string;
929277 19
e43149 20 #define OUTPUT_BUFFER_SIZE 512
929277 21
JS 22 int RunCollect=0;
e43149 23 std::unique_ptr<UdpTransmitSocket> transmitSocket;
d6f0ba 24 bool use_text = false;
18a72e 25 int pollPeriod;
929277 26
JS 27
28 void* Collect(REND_STRUCT* prs){
3dfc1c 29     int iter = 0;
929277 30     CG4Trk* pTrk = prs->pTrk;
JS 31     while (RunCollect){
32         // periodically check for new hubs coming on line
3dfc1c 33         if(iter % 1000 == 0) {
929277 34             int numHubs;
JS 35             if (prs->tot_hubs_on_system!=prs->numHub){
36                 prs->numHub=numHubs=prs->tot_hubs_on_system=pTrk->UpdateHubs();
37                 int colorInd=0;
38                 if (prs->hubList) {
39                     delete[] prs->hubList;
40                 }
41                 prs->hubList=new CG4Hub[numHubs];
42                 int* hubIds=new int[numHubs];
43                 pTrk->GetHubList(hubIds,numHubs);
44                 for (int i=0;i<numHubs;i++){
45                     prs->hubList[i].SetId(hubIds[i]);
46                 }
47                 delete[] hubIds;
48             }
49
50         }
3dfc1c 51         iter++;
929277 52         prs->hubreadmap=0;
JS 53         prs->tot_hubs_on_system=prs->pTrk->GetHubsPno(prs);
e43149 54         char buffer[OUTPUT_BUFFER_SIZE];
JS 55
929277 56         for (int i=0;i<prs->numHub;i++){
ee3cd1 57             osc::OutboundPacketStream p( buffer, OUTPUT_BUFFER_SIZE );
95f3f3 58             if(!use_text) {
JS 59                 p << osc::BeginBundleImmediate;
60             }
7fde24 61             p << osc::BeginMessage( (string("/g4/hub/") + std::to_string(prs->hubList[i].GetId()) + string("/digio")).c_str());
JS 62             std::ostringstream ss;
63             if(use_text) {
64                 ss << prs->hubList[i].GetDigIOData();
65                 auto res = ss.str();
66                 p << res.c_str();
67             } else {
68                 p << osc::int64(prs->hubList[i].GetDigIOData());
69             }
70             p << osc::EndMessage;
3dfc1c 71             auto dat = prs->hubList[i].GetPnoData();
d05d79 72             for(int j=0;j<G4_SENSORS_PER_HUB;j++) {
JS 73                 if(!prs->hubList[i].IsSenActive(j)){
74                     continue;
75                 }
a59e92 76                 p << osc::BeginMessage( (string("/g4/hub/") + std::to_string(prs->hubList[i].GetId()) + string("/") + std::to_string(j)).c_str());
d05d79 77                 std::ostringstream ss;
JS 78                 if(use_text) {
3dfc1c 79                     ss << "[" << dat[j][0] << "," << dat[j][1] << "," << dat[j][2] << "," << dat[j][3] << "," << dat[j][4] << "," << dat[j][5] << "]";
d05d79 80                     auto res = ss.str();
JS 81                     p << res.c_str();
82                 } else {
3dfc1c 83                     p << dat[j][0] << dat[j][1] << dat[j][2] << dat[j][3] << dat[j][4] << dat[j][5];
d05d79 84                 }
JS 85                 p << osc::EndMessage;
d6f0ba 86             }
d05d79 87             if(!use_text) {
JS 88                 p << osc::EndBundle;
89             }
90             transmitSocket->Send( p.Data(), p.Size() );
929277 91         }
b21ff0 92         std::this_thread::sleep_for(std::chrono::milliseconds(pollPeriod));
929277 93     }
JS 94
69b11a 95     g4_close_tracker();
929277 96     return NULL;
3dfc1c 97 }
JS 98
99 void sighandler(int s){
100     printf("Caught signal %d\n",s);
69b11a 101     RunCollect=0;
929277 102 }
JS 103
104 int main(int argc,char* argv[]){
105
106   CG4Trk trk;
107   REND_STRUCT rs;
e9b870 108   string cfgFile;
e43149 109   string oscSinkAddr;
JS 110   int oscSinkPort;
929277 111
e9b870 112   auto cli = (
d6f0ba 113     option("-t", "--text").set(use_text).doc("send strings in osc messages instead of a float bundle"),
e9b870 114         value("c4g configuration file", cfgFile),
e43149 115         value("osc sink address", oscSinkAddr),
c3b1e1 116         integer("osc sink port", oscSinkPort),
JS 117         integer("polling period in ms (imprecise). You realistically won't need less than 8 ms.", pollPeriod)
e9b870 118   );
929277 119
c3b1e1 120   if(!parse(argc, argv, cli) || pollPeriod <= 0) {
e9b870 121       cout << make_man_page(cli, argv[0]);
JS 122     return 1;
123   }
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