Commit | Autor | Alter
|
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 |
|