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