vrpn
07.33
Virtual Reality Peripheral Network
|
Go to the documentation of this file.
64 inline static bool wm_isnan(
const double x) {
77 const double fovX = Q_DEG_TO_RAD(43.0),
fovY = Q_DEG_TO_RAD(32.00);
86 #define MAKE_IDENTITY_QUAT(dest) \
87 dest[0] = dest[1] = dest[2] = 0; dest[3] = 1
91 #define MAKE_NULL_VEC(dest) \
92 dest[0] = dest[1] = dest[2] = 0
102 d_update_interval(update_rate ? (1.0 / update_rate) : 1.0 / 60.0),
103 d_blobDistance(led_spacing),
104 d_flipState(FLIP_UNKNOWN),
112 if (wiimote == NULL) {
114 fprintf(stderr,
"vrpn_Tracker_WiimoteHead: "
115 "Can't start without a valid specified Wiimote device!");
185 fprintf(stderr,
"vrpn_Tracker_WiimoteHead: "
186 "Can't open Analog %s\n",
d_name);
193 fprintf(stderr,
"vrpn_Tracker_WiimoteHead: "
194 "Can't setup change handler on Analog %s\n",
d_name);
230 q_xyz_quat_type newPose;
271 fprintf(stderr,
"vrpn_Tracker_WiimoteHead: "
272 "cannot write message: tossing\n");
275 fprintf(stderr,
"vrpn_Tracker_WiimoteHead: "
276 "No valid connection\n");
293 fprintf(stderr,
"vrpn_Tracker_WiimoteHead: "
294 "got first report from Wiimote!\n");
300 for (i = 0; i < 4; i++) {
301 firstchan = i * 3 + 4;
304 if (info.channel[firstchan] > 0
305 && info.channel[firstchan + 1] > 0
306 && info.channel[firstchan + 2] > 0) {
307 wh->
d_vX[i] = info.channel[firstchan];
308 wh->
d_vY[i] = info.channel[firstchan + 1];
309 wh->
d_vSize[i] = info.channel[firstchan + 2];
319 bool newgrav =
false;
322 for (i = 0; i < 3; i++) {
323 if (info.channel[1 + i] != wh->
d_vGrav[i]) {
335 for (i = 0; i < 3; i++) {
336 wh->
d_vGrav[i] = info.channel[1 + i];
342 wh->vrpn_Tracker::timestamp = info.msg_time;
361 q_vec_type movingAvg = Q_NULL_VECTOR;
363 q_vec_copy(movingAvg,
d_vGrav);
366 q_vec_scale(movingAvg, 0.33333, movingAvg);
372 q_vec_type regulargravity = Q_NULL_VECTOR;
373 regulargravity[2] = 1;
394 double X0, X1, Y0, Y1;
412 const double dx = X0 - X1;
413 const double dy = Y0 - Y1;
414 const double dist = sqrt(dx * dx + dy * dy);
422 newPose.xyz[2] = headDist;
427 const double avgX = (X0 + X1) / 2.0;
428 const double avgY = (Y0 + Y1) / 2.0;
434 if (wm_isnan(avgX)) {
435 std::cerr <<
"NaN detected in avgX: X0 = " << X0 <<
", X1 = " << X1 << std::endl;
439 if (wm_isnan(avgY)) {
440 std::cerr <<
"NaN detected in avgY: Y0 = " << Y0 <<
", Y1 = " << Y1 << std::endl;
451 newPose.xyz[0] = headDist * (avgX -
xResSensor / 2) / bHoriz;
452 newPose.xyz[1] = headDist * (avgY -
yResSensor / 2) / bVert;
455 q_from_euler(newPose.quat, rz, ry, rx);
467 q_vec_type upVec = {0, 1, 0};
474 fprintf(stderr,
"vrpn_Tracker_WiimoteHead: d_flipState = FLIP_180\n");
481 fprintf(stderr,
"vrpn_Tracker_WiimoteHead: d_flipState = FLIP_NORMAL\n");
vrpn_Tracker interface provided by processing Wii Remote data for head tracking.
const double fovX
Field of view experimentally determined at Iowa State University March 2010.
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.
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...
const double d_blobDistance
distance between LEDs on glasses, in meters
const char * vrpn_got_connection
virtual void reset()
reset pose, gravity transform, and cached points and gravity
virtual int encode_to(char *buf)
const char * d_name
Tracker device name.
virtual ~vrpn_Tracker_WiimoteHead()
destructor
double vrpn_TimevalDurationSeconds(struct timeval endT, struct timeval startT)
Return the number of seconds between startT and endT as a floating-point value.
void setup_wiimote()
set up connection to wiimote-like analog device
q_xyz_quat_type d_gravityXform
Gravity correction transformation.
void _reset_gravity()
reset gravity transform and cached gravity vectors
void _reset_points()
reset cached points, point count, and flip state,
q_vec_type d_vGravPenultimate
static int VRPN_CALLBACK VRPN_CALLBACK handle_connection(void *, vrpn_HANDLERPARAM)
Callback triggered when a new client connects to the tracker.
void _update_gravity_moving_avg()
based on cached gravity data, use a moving average to update the tracker's stored gravity transform.
#define MAKE_IDENTITY_QUAT(dest)
Utility function to set a quat equal to the identity rotation.
#define MAKE_NULL_VEC(dest)
Utility function to set a 3-vector equal to the zero vector.
vrpn_Analog_Remote * d_ana
Source of analog data, traditionally vrpn_WiiMote Must present analog channels in this order:
const double d_update_interval
maximum time between updates, in seconds
const vrpn_uint32 vrpn_CONNECTION_LOW_LATENCY
vrpn_Connection * d_connection
Connection that this object talks to.
q_xyz_quat_type d_currentPose
Current pose estimate.
virtual vrpn_int32 register_message_type(const char *name)
bool d_contact
Flag: Have we received the first message from the Wiimote?
bool _should_report(double elapsedInterval) const
return true if we have new data or max time elapsed
This structure is what is passed to a vrpn_Connection message callback.
void update_pose()
function to drive the full pose update process
vrpn_int32 d_sender_id
Sender ID registered with the connection.
void _update_2_LED_pose(q_xyz_quat_type &newPose)
Create tracker-relative pose estimate based on sensor location of 2 tracked points.
bool _have_gravity() const
return true if our gravity values look like real data
const double cvtDistToAngle
q_vec_type d_vGravAntepenultimate
bool d_updated
Flag: Have we received updated Wiimote data since last report?
virtual int unregister_change_handler(void *userdata, vrpn_ANALOGCHANGEHANDLER handler)
virtual void mainloop()
Called once through each main loop iteration to handle updates. Remote object mainloop() should call ...
virtual void mainloop()
VRPN mainloop function.
Generic connection class not specific to the transport mechanism.
#define vrpn_gettimeofday
void _update_flip_state()
If flip state is unknown, set flip state appropriately.
bool d_lock
Flag: Does the tracking algorithm report a lock?
void report()
Pack and send tracker report.
void _convert_pose_to_tracker()
Set the vrpn_Tracker position and rotation to that indicated by our d_currentPose;.
void _reset_pose()
reset current pose, last report time, and tracker pose
vrpn_Tracker_WiimoteHead(const char *name, vrpn_Connection *trackercon, const char *wiimote, float update_rate, float led_spacing=0.145)
constructor
bool d_gravDirty
Flag: Have we received updated gravity data since last gravity update?
virtual int register_change_handler(void *userdata, vrpn_ANALOGCHANGEHANDLER handler)
FlipState d_flipState
Whether we need to flip the order of the tracked points before calculating a pose.
void server_mainloop(void)
Handles functions that all servers should provide in their mainloop() (ping/pong, for example) Should...
static void VRPN_CALLBACK VRPN_CALLBACK handle_analog_update(void *userdata, const vrpn_ANALOGCB info)
Callback triggered when our data source issues an update.
struct timeval d_prevtime
Time of last tracker report issued.
Provides a tracker device given data from a Wii Remote and LED glasses.