Panda3D
 All Classes Functions Variables Enumerations
vrpnTracker.cxx
1 // Filename: vrpnTracker.cxx
2 // Created by: drose (25Jan01)
3 //
4 ////////////////////////////////////////////////////////////////////
5 //
6 // PANDA 3D SOFTWARE
7 // Copyright (c) Carnegie Mellon University. All rights reserved.
8 //
9 // All use of this software is subject to the terms of the revised BSD
10 // license. You should have received a copy of this license along
11 // with this source code in a file named "LICENSE."
12 //
13 ////////////////////////////////////////////////////////////////////
14 
15 #include "vrpnTracker.h"
16 #include "vrpnTrackerDevice.h"
17 #include "vrpnClient.h"
18 #include "config_vrpn.h"
19 
20 #include "indent.h"
21 
22 #include <algorithm>
23 
24 ////////////////////////////////////////////////////////////////////
25 // Function: VrpnTracker::Constructor
26 // Access: Public
27 // Description:
28 ////////////////////////////////////////////////////////////////////
29 VrpnTracker::
30 VrpnTracker(const string &tracker_name, vrpn_Connection *connection) :
31  _tracker_name(tracker_name)
32 {
33  _tracker = new vrpn_Tracker_Remote(_tracker_name.c_str(), connection);
34 
35  _tracker->register_change_handler((void*)this, &vrpn_position_callback);
36  _tracker->register_change_handler((void*)this, &vrpn_velocity_callback);
37  _tracker->register_change_handler((void*)this, &vrpn_acceleration_callback);
38 }
39 
40 ////////////////////////////////////////////////////////////////////
41 // Function: VrpnTracker::Destructor
42 // Access: Public
43 // Description:
44 ////////////////////////////////////////////////////////////////////
45 VrpnTracker::
46 ~VrpnTracker() {
47  delete _tracker;
48 }
49 
50 ////////////////////////////////////////////////////////////////////
51 // Function: VrpnTracker::mark
52 // Access: Public
53 // Description: Adds the indicated VrpnTrackerDevice to the list of
54 // devices that are sharing this VrpnTracker.
55 ////////////////////////////////////////////////////////////////////
56 void VrpnTracker::
58  if (vrpn_cat.is_debug()) {
59  vrpn_cat.debug() << *this << " marking " << *device << "\n";
60  }
61  _devices.push_back(device);
62 }
63 
64 ////////////////////////////////////////////////////////////////////
65 // Function: VrpnTracker::unmark
66 // Access: Public
67 // Description: Removes the indicated VrpnTrackerDevice from the list
68 // of devices that are sharing this VrpnTracker.
69 ////////////////////////////////////////////////////////////////////
70 void VrpnTracker::
72  if (vrpn_cat.is_debug()) {
73  vrpn_cat.debug() << *this << " unmarking " << *device << "\n";
74  }
75 
76  Devices::iterator di =
77  find(_devices.begin(), _devices.end(), device);
78 
79  if (di != _devices.end()) {
80  _devices.erase(di);
81  }
82 }
83 
84 ////////////////////////////////////////////////////////////////////
85 // Function: VrpnTracker::output
86 // Access: Public
87 // Description:
88 ////////////////////////////////////////////////////////////////////
89 void VrpnTracker::
90 output(ostream &out) const {
91  out << _tracker_name;
92 }
93 
94 ////////////////////////////////////////////////////////////////////
95 // Function: VrpnTracker::write
96 // Access: Public
97 // Description:
98 ////////////////////////////////////////////////////////////////////
99 void VrpnTracker::
100 write(ostream &out, int indent_level) const {
101  indent(out, indent_level)
102  << get_tracker_name() << " ("
103  << _devices.size() << " devices)\n";
104 }
105 
106 ////////////////////////////////////////////////////////////////////
107 // Function: VrpnTracker::vrpn_position_callback
108 // Access: Private, Static
109 // Description: Receives the tracker positional data from the VRPN
110 // code and sends it to any interested
111 // VrpnTrackerDevices.
112 ////////////////////////////////////////////////////////////////////
113 void VRPN_CALLBACK VrpnTracker::
114 vrpn_position_callback(void *userdata, const vrpn_TRACKERCB info) {
115  VrpnTracker *self = (VrpnTracker *)userdata;
116  if (vrpn_cat.is_spam()) {
117  vrpn_cat.spam()
118  << *self << " position_callback\n";
119  }
120 
121  Devices::iterator di;
122  for (di = self->_devices.begin(); di != self->_devices.end(); ++di) {
123  VrpnTrackerDevice *device = (*di);
124  if (device->get_sensor() == info.sensor &&
125  device->get_data_type() == VrpnTrackerDevice::DT_position) {
126  device->acquire();
127  device->_data.set_time(VrpnClient::convert_to_secs(info.msg_time));
128  device->_data.set_pos(LPoint3(info.pos[0], info.pos[1], info.pos[2]));
129  device->_data.set_orient(LOrientation(info.quat[3], info.quat[0], info.quat[1], info.quat[2]));
130  device->unlock();
131  }
132  }
133 }
134 
135 ////////////////////////////////////////////////////////////////////
136 // Function: VrpnTracker::vrpn_velocity_callback
137 // Access: Private, Static
138 // Description: Receives the tracker velocity data from the VRPN
139 // code and sends it to any interested
140 // VrpnTrackerDevices.
141 ////////////////////////////////////////////////////////////////////
142 void VRPN_CALLBACK VrpnTracker::
143 vrpn_velocity_callback(void *userdata, const vrpn_TRACKERVELCB info) {
144  VrpnTracker *self = (VrpnTracker *)userdata;
145  if (vrpn_cat.is_spam()) {
146  vrpn_cat.spam()
147  << *self << " velocity_callback\n";
148  }
149 
150  Devices::iterator di;
151  for (di = self->_devices.begin(); di != self->_devices.end(); ++di) {
152  VrpnTrackerDevice *device = (*di);
153  if (device->get_sensor() == info.sensor &&
154  device->get_data_type() == VrpnTrackerDevice::DT_velocity) {
155  device->acquire();
156  device->_data.set_time(VrpnClient::convert_to_secs(info.msg_time));
157  device->_data.set_pos(LPoint3(info.vel[0], info.vel[1], info.vel[2]));
158  device->_data.set_orient(LOrientation(info.vel_quat[3], info.vel_quat[0],
159  info.vel_quat[1], info.vel_quat[2]));
160  device->_data.set_dt(info.vel_quat_dt);
161  device->unlock();
162  }
163  }
164 }
165 
166 ////////////////////////////////////////////////////////////////////
167 // Function: VrpnTracker::vrpn_acceleration_callback
168 // Access: Private, Static
169 // Description: Receives the tracker acceleration data from the VRPN
170 // code and sends it to any interested
171 // VrpnTrackerDevices.
172 ////////////////////////////////////////////////////////////////////
173 void VRPN_CALLBACK VrpnTracker::
174 vrpn_acceleration_callback(void *userdata, const vrpn_TRACKERACCCB info) {
175  VrpnTracker *self = (VrpnTracker *)userdata;
176  if (vrpn_cat.is_spam()) {
177  vrpn_cat.spam()
178  << *self << " acceleration_callback\n";
179  }
180 
181  Devices::iterator di;
182  for (di = self->_devices.begin(); di != self->_devices.end(); ++di) {
183  VrpnTrackerDevice *device = (*di);
184  if (device->get_sensor() == info.sensor &&
185  device->get_data_type() == VrpnTrackerDevice::DT_acceleration) {
186  device->acquire();
187  device->_data.set_time(VrpnClient::convert_to_secs(info.msg_time));
188  device->_data.set_pos(LPoint3(info.acc[0], info.acc[1], info.acc[2]));
189  device->_data.set_orient(LOrientation(info.acc_quat[3], info.acc_quat[0],
190  info.acc_quat[1], info.acc_quat[2]));
191  device->_data.set_dt(info.acc_quat_dt);
192  device->unlock();
193  }
194  }
195 }
This is a unit quaternion representing an orientation.
Definition: lorientation.h:92
int get_sensor() const
Returns the particular sensor index that this device wants to hear about from the VrpnTracker...
void unmark(VrpnTrackerDevice *device)
Removes the indicated VrpnTrackerDevice from the list of devices that are sharing this VrpnTracker...
Definition: vrpnTracker.cxx:71
This is a three-component point in space (as opposed to a three-component vector, which represents a ...
Definition: lpoint3.h:99
void unlock()
Releases the mutex associated with this particular device.
Definition: clientDevice.I:90
This is the actual interface to a particular VRPN tracker object, and all of its sensors.
Definition: vrpnTracker.h:41
The Panda interface to a VRPN tracker.
DataType get_data_type() const
Returns the type of data this device represents from the VrpnTracker.
void mark(VrpnTrackerDevice *device)
Adds the indicated VrpnTrackerDevice to the list of devices that are sharing this VrpnTracker...
Definition: vrpnTracker.cxx:57
const string & get_tracker_name() const
Returns the name of the tracker device that was used to create this VrpnTracker.
Definition: vrpnTracker.I:22
void acquire()
Grabs the mutex associated with this particular device.
Definition: clientDevice.I:76
static double convert_to_secs(struct timeval msg_time)
Little inline function to convert a struct timeval to only seconds.
Definition: vrpnClient.I:59