vrpn 07.35
Virtual Reality Peripheral Network
Loading...
Searching...
No Matches
vrpn_Tracker_Colibri.C
Go to the documentation of this file.
1
2//
3// Name: vrpn_Tracker_Colibri.C
4//
5// Author: Dmitry Mastykin
6//
7// Description: VRPN tracker class for Trivisio Colibri device (based on ColibriAPI).
8//
10#include <stdlib.h>
11#include <math.h>
12#include "quat.h"
14
15#ifdef VRPN_USE_COLIBRIAPI
16
17vrpn_Tracker_Colibri::vrpn_Tracker_Colibri(const char* name, vrpn_Connection* c,
18 const char* path, int Hz, bool report_a_w)
19 : vrpn_Tracker(name, c), nw(NULL), report_a_w(report_a_w)
20{
21 TCResult result;
22 printf("ColibriAPI version: %s\n", TC_APIVersion());
23
24 if (TC_Initialize(colibri_api_strings) != TCR_SUCCESS) {
25 printf("Initialize ColibriAPI failed!\n");
26 exit(EXIT_FAILURE);
27 }
28
29 if (!path) // No configuration file
30 result = TC_NewFromHardware(&nw, TCO_NEW_WIRED | TCO_NEW_WIRELESS);
31 else {
32 TC_New(&nw, 0);
33 result = TC_LoadConf(nw, path);
34 if (result == TCR_SUCCESS)
35 result = TC_New(&nw, TCO_NEW_WIRED | TCO_NEW_WIRELESS);
36 }
37
38 bool ok;
39 for (ok = false; !ok; ok = true) {
40 if (result == TCR_FILE) {
41 printf("File %s not found or bad format.\n", path);
42 break;
43 }
44
45 // Query the number of connected devices
46 num_sensors = TC_GetNodeInfo(nw, 0, NULL);
47 printf("%d Colibri sensors found\n", num_sensors);
48
49 if (result != TCR_SUCCESS) {
50 printf("%s not found (error %d).\n", TC_GetFailedID(nw)[0]?TC_GetFailedID(nw):"Device", result);
51 if (result == TCR_IO_REMOTE) {
52 printf("Dongle will scan for wireless Colibri next 20 seconds...\n"
53 "If sensor is off - just turn it on; if you think it's on another channel: "
54 "put it in scanning mode (push button for 2 seconds, LED will flash fast).\n"
55 "Then restart server.\n");
56 const char (*list)[8];
57 unsigned int nodes_found;
58 int gates = TC_GetGateInfo(nw, 0, NULL);
59 for (int j = 0; j < gates; ++j)
60 TC_Discover(nw, j, &list, &nodes_found);
61 }
62 break;
63 }
64
65 TC_SaveConf(nw, "./current.conf");
66 for (int i = 0; i < num_sensors; ++i) {
67 TCInfo info;
68 TC_GetNodeInfo(nw, i, &info);
69 printf("%d: %s (FW %d.%.3d)\n", i, info.ID, info.FW_ver/1000, info.FW_ver%1000);
70 }
71
72 if (!path) { // No configuration file
73 result = TC_QuickConfig(nw, TCS_ORIENT | (report_a_w ? TCS_ACC|TCS_GYR : 0));
74 if (result != TCR_SUCCESS) {
75 printf("Colibri sensor error.\n");
76 break;
77 }
78 }
79
80 // Start the devices
81 vel_quat_dt = 1.f/Hz;
82 result = TC_Run(nw, Hz);
83 if (result == TCR_SUCCESS) {
84 printf("Network started...\n");
85 } else {
86 printf("Failed to start network.\n");
87 break;
88 }
89 }
90
91 if (!ok) {
92 TC_Close(nw);
93 exit(EXIT_FAILURE);
94 }
95
96 // VRPN stuff
97 register_server_handlers();
98}
99
100vrpn_Tracker_Colibri::~vrpn_Tracker_Colibri()
101{
102 if (!nw)
103 return;
104
105 if (TCR_SUCCESS == TC_Stop(nw))
106 printf("Network stopped.\n");
107 else
108 printf("Failed to stop network.\n");
109
110 TC_Close(nw);
111}
112
113void vrpn_Tracker_Colibri::mainloop()
114{
115 // Call the generic server mainloop, since we are a server
116 server_mainloop();
117
118 // Get latest data
119 get_report();
120}
121
122void vrpn_Tracker_Colibri::get_report()
123{
124 //vrpn_gettimeofday(&timestamp, NULL);
125
126 const TCSample **data;
127 while (TCR_SUCCESS == TC_GetNextData(nw, &data)) {
128 for (int i = 0; i < num_sensors; ++i) {
129 if (data[i]) {
130 // The sensor number
131 d_sensor = i;
132
133 // Time of the report
134 long tts, ttu;
135 tts = data[i]->t / 10000;
136 ttu = (data[i]->t - tts * 10000) * 100;
137 timestamp.tv_sec = tts;
138 timestamp.tv_usec = ttu;
139
140 // No need to fill in position, as we don't get position information
141
142 // Orientation of the sensor
143 d_quat[Q_X] = data[i]->q[1];
144 d_quat[Q_Y] = data[i]->q[2];
145 d_quat[Q_Z] = data[i]->q[3];
146 d_quat[Q_W] = data[i]->q[0];
147 q_conjugate(d_quat, d_quat); // VRPN defines a right-handed coordinate system
148
149 if (report_a_w) {
150 // Acceleration of the sensor
151 acc[0] = data[i]->a[0];
152 acc[1] = data[i]->a[1];
153 acc[2] = data[i]->a[2];
154
155 // Delta Orientation of the sensor
156 const vrpn_float64 x = data[i]->g[0] * vel_quat_dt;
157 const vrpn_float64 y = data[i]->g[1] * vel_quat_dt;
158 const vrpn_float64 z = data[i]->g[2] * vel_quat_dt;
159 const vrpn_float64 angle = sqrt(x*x + y*y + z*z); //module of angular velocity
160 if (angle > 0.0) {
161 vel_quat[Q_X] = x*sin(angle/2.0f)/angle;
162 vel_quat[Q_Y] = y*sin(angle/2.0f)/angle;
163 vel_quat[Q_Z] = z*sin(angle/2.0f)/angle;
164 vel_quat[Q_W] = cos(angle/2.0f);
165 } else { //to avoid illegal expressions
166 vel_quat[Q_X] = vel_quat[Q_Y] = vel_quat[Q_Z] = 0.0f;
167 vel_quat[Q_W] = 1.0f;
168 }
169 }
170
171 // Send the data
172 send_report();
173 }
174 }
175 }
176}
177
178void vrpn_Tracker_Colibri::send_report()
179{
180 if (d_connection) {
181 char msgbuf[1000];
182
183 int len = encode_to(msgbuf);
184 if (d_connection->pack_message(len, timestamp, position_m_id, d_sender_id, msgbuf, vrpn_CONNECTION_LOW_LATENCY))
185 fprintf(stderr, "Tracker: cannot write message: tossing\n");
186
187 if (report_a_w) {
188 len = encode_acc_to(msgbuf);
189 if (d_connection->pack_message(len, timestamp, accel_m_id, d_sender_id, msgbuf, vrpn_CONNECTION_LOW_LATENCY))
190 fprintf(stderr,"Tracker: cannot write message: tossing\n");
191
192 len = encode_vel_to(msgbuf);
193 if (d_connection->pack_message(len, timestamp, velocity_m_id, d_sender_id, msgbuf, vrpn_CONNECTION_LOW_LATENCY))
194 fprintf(stderr,"Tracker: cannot write message: tossing\n");
195 }
196 }
197}
198
199#endif //VRPN_USE_COLIBRIAPI
200
Generic connection class not specific to the transport mechanism.
const vrpn_uint32 vrpn_CONNECTION_LOW_LATENCY