vrpn 07.35
Virtual Reality Peripheral Network
 
Loading...
Searching...
No Matches
vrpn_Poser_Analog.C
Go to the documentation of this file.
1#include "vrpn_Analog.h" // for vrpn_CHANNEL_MAX
2#include "vrpn_Analog_Output.h" // for vrpn_Analog_Output_Remote
3#include "vrpn_Connection.h" // for vrpn_HANDLERPARAM, etc
4#include "vrpn_Poser_Analog.h"
5#include "vrpn_Shared.h" // for vrpn_unbuffer, timeval, etc
6
8{
9 // set workspace values to defaults
10 for (int i = 0; i < 3; i++) {
11 pos_min[i] = vel_min[i] = -1;
12 pos_max[i] = vel_max[i] = 1;
13 pos_rot_min[i] = vel_rot_min[i] = -45;
14 pos_rot_max[i] = vel_rot_max[i] = 45;
15 }
16}
17
19{
20 // If the name is NULL, we're done.
21 if (full->axis.ana_name == NULL) {
22 return 0;
23 }
24
25 // Create the Analog Output Remote
26 // If the name starts with the '*' character, use the server
27 // connection rather than making a new one.
28 if (full->axis.ana_name != NULL) {
29 if (full->axis.ana_name[0] == '*') {
30 try {
31 full->ana = new vrpn_Analog_Output_Remote(&(full->axis.ana_name[1]),
33 } catch (...) {
34 fprintf(stderr, "vrpn_Poser_Analog: Out of memory\n");
35 return false;
36 }
37 } else {
38 try {
39 full->ana = new vrpn_Analog_Output_Remote(full->axis.ana_name);
40 } catch (...) {
41 fprintf(stderr, "vrpn_Poser_Analog: Out of memory\n");
42 return false;
43 }
44 }
45 }
46 else {
47 full->ana = NULL;
48 fprintf(stderr,
49 "vrpn_Poser_Analog: Can't open Analog: No name given\n");
50 return false;
51 }
52 return true;
53}
54
57 bool act_as_tracker)
58 : vrpn_Poser(name, c)
59 , vrpn_Tracker(name, c)
60 , d_act_as_tracker(act_as_tracker)
61{
62 int i;
63
64 // register_server_handlers();
65
66 // Make sure that we have a valid connection
67 if (d_connection == NULL) {
68 fprintf(stderr, "vrpn_Poser_Analog: No connection\n");
69 return;
70 }
71
72 // Register a handler for the position change callback for this device
74 this, d_sender_id)) {
75 fprintf(stderr, "vrpn_Poser_Analog: can't register position handler\n");
76 d_connection = NULL;
77 }
78
79 // Register a handler for the velocity change callback for this device
82 fprintf(stderr, "vrpn_Poser_Analog: can't register velocity handler\n");
83 d_connection = NULL;
84 }
85
86 // Set up the axes
87 x.axis = p->x;
88 y.axis = p->y;
89 z.axis = p->z;
90 rx.axis = p->rx;
91 ry.axis = p->ry;
92 rz.axis = p->rz;
93
94 x.ana = y.ana = z.ana = NULL;
95 rx.ana = ry.ana = rz.ana = NULL;
96
97 x.value = y.value = z.value = 0.0;
98 rx.value = ry.value = rz.value = 0.0;
99
100 x.pa = this;
101 y.pa = this;
102 z.pa = this;
103 rx.pa = this;
104 ry.pa = this;
105 rz.pa = this;
106
107 //--------------------------------------------------------------------
108 // Open analog remotes for any channels that have non-NULL names.
109 // If the name starts with the "*" character, use tracker
110 // connection rather than getting a new connection for it.
117
118 // Set up the workspace max and min values
119 for (i = 0; i < 3; i++) {
120 p_pos_min[i] = p->pos_min[i];
121 p_pos_max[i] = p->pos_max[i];
122 p_vel_min[i] = p->vel_min[i];
123 p_vel_max[i] = p->vel_max[i];
124 p_pos_rot_min[i] = p->pos_rot_min[i];
125 p_pos_rot_max[i] = p->pos_rot_max[i];
126 p_vel_rot_min[i] = p->vel_rot_min[i];
127 p_vel_rot_max[i] = p->vel_rot_max[i];
128 }
129
130 // Check the pose for each channel against the max and min values of the
131 // workspace
132 // and set it to the location closest to the origin.
133 p_pos[0] = p_pos[1] = p_pos[2] = 0.0;
134 p_quat[0] = p_quat[1] = p_quat[2] = 0.0;
135 p_quat[3] = 1.0;
136 for (i = 0; i < 3; i++) {
137 if (p_pos[i] < p_pos_min[i]) {
138 p_pos[i] = p_pos_min[i];
139 }
140 else if (p_pos[i] > p_pos_max[i]) {
141 p_pos[i] = p_pos_max[i];
142 }
143 }
144}
145
147
149{
150 // Call generic server mainloop, since we are a server
152
153 // Call the Analog outputs' mainloops
154 if (x.ana != NULL) {
155 x.ana->mainloop();
156 };
157 if (y.ana != NULL) {
158 y.ana->mainloop();
159 };
160 if (z.ana != NULL) {
161 z.ana->mainloop();
162 };
163 if (rx.ana != NULL) {
164 rx.ana->mainloop();
165 };
166 if (ry.ana != NULL) {
167 ry.ana->mainloop();
168 };
169 if (rz.ana != NULL) {
170 rz.ana->mainloop();
171 };
172}
173
176{
178 const char* params = (p.buffer);
179 int i;
180 bool outside_bounds = false;
181
182 // Fill in the parameters to the poser from the message
183 if (p.payload_len != (7 * sizeof(vrpn_float64))) {
184 fprintf(stderr, "vrpn_Poser_Server: change message payload error\n");
185 fprintf(stderr, " (got %d, expected %d)\n", p.payload_len,
186 static_cast<int>(7 * sizeof(vrpn_float64)));
187 return -1;
188 }
189 me->p_timestamp = p.msg_time;
190
191 for (i = 0; i < 3; i++) {
192 vrpn_unbuffer(&params, &me->p_pos[i]);
193 }
194 for (i = 0; i < 4; i++) {
195 vrpn_unbuffer(&params, &me->p_quat[i]);
196 }
197
198 // Check the pose against the max and min values of the workspace
199 for (i = 0; i < 3; i++) {
200 if (me->p_pos[i] < me->p_pos_min[i]) {
201 me->p_pos[i] = me->p_pos_min[i];
202 outside_bounds = true;
203 }
204 else if (me->p_pos[i] > me->p_pos_max[i]) {
205 me->p_pos[i] = me->p_pos_max[i];
206 outside_bounds = true;
207 }
208 }
209
210 // Update the analog values based on the request we just got.
211 if (!me->update_Analog_values()) {
212 fprintf(stderr, "vrpn_Poser_Analog: Error updating Analog values\n");
213 }
214
215 if (me->d_act_as_tracker) {
216 // Tell the client where we actually went (clipped position and
217 // orientation).
218 // using sensor 0 as the one to use to report.
219 me->d_sensor = 0;
220 me->pos[0] = me->p_pos[0];
221 me->pos[1] = me->p_pos[1];
222 me->pos[2] = me->p_pos[2];
223 me->d_quat[0] = me->p_quat[0];
224 me->d_quat[1] = me->p_quat[1];
225 me->d_quat[2] = me->p_quat[2];
226 me->d_quat[3] = me->p_quat[3];
227 vrpn_gettimeofday(&me->vrpn_Tracker::timestamp, NULL);
228 char msgbuf[1000];
229 vrpn_int32 len;
230 len = me->vrpn_Tracker::encode_to(msgbuf);
231 if (me->d_connection->pack_message(
232 len, me->vrpn_Tracker::timestamp, me->position_m_id,
234 fprintf(stderr, "vrpn_Poser_Analog::handle_change_message(): can't "
235 "write message: tossing\n");
236 return -1;
237 }
238 }
239
240 return 0;
241}
242
245{
247 const char* params = (p.buffer);
248 int i;
249 bool outside_bounds = false;
250
251 // Fill in the parameters to the poser from the message
252 if (p.payload_len != (8 * sizeof(vrpn_float64))) {
253 fprintf(stderr, "vrpn_Poser_Server: velocity message payload error\n");
254 fprintf(stderr, " (got %d, expected %d)\n", p.payload_len,
255 static_cast<int>(8 * sizeof(vrpn_float64)));
256 return -1;
257 }
258 me->p_timestamp = p.msg_time;
259
260 for (i = 0; i < 3; i++) {
261 vrpn_unbuffer(&params, &me->p_vel[i]);
262 }
263 for (i = 0; i < 4; i++) {
264 vrpn_unbuffer(&params, &me->p_vel_quat[i]);
265 }
266 vrpn_unbuffer(&params, &me->p_vel_quat_dt);
267
268 // Check the velocity against the max and min values of the workspace
269 for (i = 0; i < 3; i++) {
270 if (me->p_vel[i] < me->p_vel_min[i]) {
271 me->p_vel[i] = me->p_vel_min[i];
272 outside_bounds = true;
273 }
274 else if (me->p_vel[i] > me->p_vel_max[i]) {
275 me->p_vel[i] = me->p_vel_max[i];
276 outside_bounds = true;
277 }
278 }
279
280 // XXX Update the values now.
281
282 if (me->d_act_as_tracker) {
283 // Tell the client where we actually went (clipped position and
284 // orientation).
285 // using sensor 0 as the one to use to report.
286 me->d_sensor = 0;
287 me->vel[0] = me->p_vel[0];
288 me->vel[1] = me->p_vel[1];
289 me->vel[2] = me->p_vel[2];
290 me->vel_quat[0] = me->p_vel_quat[0];
291 me->vel_quat[1] = me->p_vel_quat[1];
292 me->vel_quat[2] = me->p_vel_quat[2];
293 me->vel_quat[3] = me->p_vel_quat[3];
294 vrpn_gettimeofday(&me->vrpn_Tracker::timestamp, NULL);
295 char msgbuf[1000];
296 vrpn_int32 len;
297 len = me->vrpn_Tracker::encode_vel_to(msgbuf);
298 if (me->d_connection->pack_message(
299 len, me->vrpn_Tracker::timestamp, me->velocity_m_id,
301 fprintf(stderr, "vrpn_Poser_Analog::handle_vel_change_message(): "
302 "can't write message: tossing\n");
303 return -1;
304 }
305 }
306
307 return 0;
308}
309
311{
312 vrpn_float64 value;
313 bool ret = true;
314
315 // XXX ONLY DOING TRANS FOR NOW...ADD ROT LATER
316 if (x.axis.channel != -1 && x.axis.channel < vrpn_CHANNEL_MAX) {
317 value = (p_pos[0] - x.axis.offset) * x.axis.scale;
318 if (x.ana != NULL) {
319 ret &= x.ana->request_change_channel_value(x.axis.channel, value);
320 }
321 }
322 if (y.axis.channel != -1 && y.axis.channel < vrpn_CHANNEL_MAX) {
323 value = (p_pos[1] - y.axis.offset) * y.axis.scale;
324 if (y.ana != NULL) {
325 ret &= y.ana->request_change_channel_value(y.axis.channel, value);
326 }
327 }
328 if (z.axis.channel != -1 && z.axis.channel < vrpn_CHANNEL_MAX) {
329 value = (p_pos[2] - z.axis.offset) * z.axis.scale;
330 if (z.ana != NULL) {
331 ret &= z.ana->request_change_channel_value(z.axis.channel, value);
332 }
333 }
334
335 return ret;
336}
int register_autodeleted_handler(vrpn_int32 type, vrpn_MESSAGEHANDLER handler, void *userdata, vrpn_int32 sender=vrpn_ANY_SENDER)
Registers a handler with the connection, and remembers to delete at destruction.
vrpn_Connection * d_connection
Connection that this object talks to.
vrpn_int32 d_sender_id
Sender ID registered with the connection.
void server_mainloop(void)
Handles functions that all servers should provide in their mainloop() (ping/pong, for example) Should...
virtual int pack_message(vrpn_uint32 len, struct timeval time, vrpn_int32 type, vrpn_int32 sender, const char *buffer, vrpn_uint32 class_of_service)
Pack a message that will be sent the next time mainloop() is called. Turn off the RELIABLE flag if yo...
vrpn_Analog_Output_Remote * ana
vrpn_float64 vel_rot_min[3]
vrpn_float64 pos_rot_max[3]
vrpn_float64 pos_rot_min[3]
vrpn_float64 vel_rot_max[3]
static int VRPN_CALLBACK handle_vel_change_message(void *userdata, vrpn_HANDLERPARAM p)
vrpn_PA_fullaxis z
vrpn_PA_fullaxis rz
vrpn_PA_fullaxis x
vrpn_PA_fullaxis ry
vrpn_Poser_Analog(const char *name, vrpn_Connection *c, vrpn_Poser_AnalogParam *p, bool act_as_tracker=false)
bool setup_channel(vrpn_PA_fullaxis *full)
vrpn_PA_fullaxis rx
virtual void mainloop()
Called once through each main loop iteration to handle updates. Remote object mainloop() should call ...
vrpn_PA_fullaxis y
static int VRPN_CALLBACK handle_change_message(void *userdata, vrpn_HANDLERPARAM p)
vrpn_float64 p_pos_min[3]
Definition vrpn_Poser.h:47
vrpn_float64 p_pos_rot_max[3]
Definition vrpn_Poser.h:47
vrpn_float64 p_vel_min[3]
Definition vrpn_Poser.h:48
vrpn_float64 p_pos[3]
Definition vrpn_Poser.h:39
vrpn_float64 p_pos_rot_min[3]
Definition vrpn_Poser.h:47
vrpn_float64 p_vel_rot_min[3]
Definition vrpn_Poser.h:48
vrpn_float64 p_vel_max[3]
Definition vrpn_Poser.h:48
vrpn_float64 p_vel_quat[4]
Definition vrpn_Poser.h:41
vrpn_Poser(const char *name, vrpn_Connection *c=NULL)
Definition vrpn_Poser.C:31
vrpn_float64 p_vel_quat_dt
Definition vrpn_Poser.h:42
vrpn_float64 p_vel_rot_max[3]
Definition vrpn_Poser.h:48
vrpn_float64 p_pos_max[3]
Definition vrpn_Poser.h:47
vrpn_int32 req_position_m_id
Definition vrpn_Poser.h:33
vrpn_float64 p_vel[3]
Definition vrpn_Poser.h:40
struct timeval p_timestamp
Definition vrpn_Poser.h:43
vrpn_int32 req_velocity_m_id
Definition vrpn_Poser.h:35
vrpn_float64 p_quat[4]
Definition vrpn_Poser.h:39
vrpn_float64 vel_quat[4]
vrpn_float64 d_quat[4]
vrpn_int32 velocity_m_id
vrpn_float64 vel[3]
vrpn_Tracker(const char *name, vrpn_Connection *c=NULL, const char *tracker_cfg_file_name=NULL)
vrpn_int32 d_sensor
vrpn_float64 pos[3]
vrpn_int32 position_m_id
This structure is what is passed to a vrpn_Connection message callback.
const char * buffer
struct timeval msg_time
#define vrpn_CHANNEL_MAX
Definition vrpn_Analog.h:16
const vrpn_uint32 vrpn_CONNECTION_LOW_LATENCY
class VRPN_API vrpn_Connection
class VRPN_API vrpn_Analog_Output_Remote
VRPN_API int vrpn_unbuffer(const char **buffer, timeval *t)
Utility routine for taking a struct timeval from a buffer that was sent as a message.
#define vrpn_gettimeofday
Definition vrpn_Shared.h:99