00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015 #include "vrpnTracker.h"
00016 #include "vrpnTrackerDevice.h"
00017 #include "vrpnClient.h"
00018 #include "config_vrpn.h"
00019
00020 #include "indent.h"
00021
00022 #include <algorithm>
00023
00024
00025
00026
00027
00028
00029 VrpnTracker::
00030 VrpnTracker(const string &tracker_name, vrpn_Connection *connection) :
00031 _tracker_name(tracker_name)
00032 {
00033 _tracker = new vrpn_Tracker_Remote(_tracker_name.c_str(), connection);
00034
00035 _tracker->register_change_handler((void*)this, &vrpn_position_callback);
00036 _tracker->register_change_handler((void*)this, &vrpn_velocity_callback);
00037 _tracker->register_change_handler((void*)this, &vrpn_acceleration_callback);
00038 }
00039
00040
00041
00042
00043
00044
00045 VrpnTracker::
00046 ~VrpnTracker() {
00047 delete _tracker;
00048 }
00049
00050
00051
00052
00053
00054
00055
00056 void VrpnTracker::
00057 mark(VrpnTrackerDevice *device) {
00058 if (vrpn_cat.is_debug()) {
00059 vrpn_cat.debug() << *this << " marking " << *device << "\n";
00060 }
00061 _devices.push_back(device);
00062 }
00063
00064
00065
00066
00067
00068
00069
00070 void VrpnTracker::
00071 unmark(VrpnTrackerDevice *device) {
00072 if (vrpn_cat.is_debug()) {
00073 vrpn_cat.debug() << *this << " unmarking " << *device << "\n";
00074 }
00075
00076 Devices::iterator di =
00077 find(_devices.begin(), _devices.end(), device);
00078
00079 if (di != _devices.end()) {
00080 _devices.erase(di);
00081 }
00082 }
00083
00084
00085
00086
00087
00088
00089 void VrpnTracker::
00090 output(ostream &out) const {
00091 out << _tracker_name;
00092 }
00093
00094
00095
00096
00097
00098
00099 void VrpnTracker::
00100 write(ostream &out, int indent_level) const {
00101 indent(out, indent_level)
00102 << get_tracker_name() << " ("
00103 << _devices.size() << " devices)\n";
00104 }
00105
00106
00107
00108
00109
00110
00111
00112
00113 void VRPN_CALLBACK VrpnTracker::
00114 vrpn_position_callback(void *userdata, const vrpn_TRACKERCB info) {
00115 VrpnTracker *self = (VrpnTracker *)userdata;
00116 if (vrpn_cat.is_spam()) {
00117 vrpn_cat.spam()
00118 << *self << " position_callback\n";
00119 }
00120
00121 Devices::iterator di;
00122 for (di = self->_devices.begin(); di != self->_devices.end(); ++di) {
00123 VrpnTrackerDevice *device = (*di);
00124 if (device->get_sensor() == info.sensor &&
00125 device->get_data_type() == VrpnTrackerDevice::DT_position) {
00126 device->acquire();
00127 device->_data.set_time(VrpnClient::convert_to_secs(info.msg_time));
00128 device->_data.set_pos(LPoint3(info.pos[0], info.pos[1], info.pos[2]));
00129 device->_data.set_orient(LOrientation(info.quat[3], info.quat[0], info.quat[1], info.quat[2]));
00130 device->unlock();
00131 }
00132 }
00133 }
00134
00135
00136
00137
00138
00139
00140
00141
00142 void VRPN_CALLBACK VrpnTracker::
00143 vrpn_velocity_callback(void *userdata, const vrpn_TRACKERVELCB info) {
00144 VrpnTracker *self = (VrpnTracker *)userdata;
00145 if (vrpn_cat.is_spam()) {
00146 vrpn_cat.spam()
00147 << *self << " velocity_callback\n";
00148 }
00149
00150 Devices::iterator di;
00151 for (di = self->_devices.begin(); di != self->_devices.end(); ++di) {
00152 VrpnTrackerDevice *device = (*di);
00153 if (device->get_sensor() == info.sensor &&
00154 device->get_data_type() == VrpnTrackerDevice::DT_velocity) {
00155 device->acquire();
00156 device->_data.set_time(VrpnClient::convert_to_secs(info.msg_time));
00157 device->_data.set_pos(LPoint3(info.vel[0], info.vel[1], info.vel[2]));
00158 device->_data.set_orient(LOrientation(info.vel_quat[3], info.vel_quat[0],
00159 info.vel_quat[1], info.vel_quat[2]));
00160 device->_data.set_dt(info.vel_quat_dt);
00161 device->unlock();
00162 }
00163 }
00164 }
00165
00166
00167
00168
00169
00170
00171
00172
00173 void VRPN_CALLBACK VrpnTracker::
00174 vrpn_acceleration_callback(void *userdata, const vrpn_TRACKERACCCB info) {
00175 VrpnTracker *self = (VrpnTracker *)userdata;
00176 if (vrpn_cat.is_spam()) {
00177 vrpn_cat.spam()
00178 << *self << " acceleration_callback\n";
00179 }
00180
00181 Devices::iterator di;
00182 for (di = self->_devices.begin(); di != self->_devices.end(); ++di) {
00183 VrpnTrackerDevice *device = (*di);
00184 if (device->get_sensor() == info.sensor &&
00185 device->get_data_type() == VrpnTrackerDevice::DT_acceleration) {
00186 device->acquire();
00187 device->_data.set_time(VrpnClient::convert_to_secs(info.msg_time));
00188 device->_data.set_pos(LPoint3(info.acc[0], info.acc[1], info.acc[2]));
00189 device->_data.set_orient(LOrientation(info.acc_quat[3], info.acc_quat[0],
00190 info.acc_quat[1], info.acc_quat[2]));
00191 device->_data.set_dt(info.acc_quat_dt);
00192 device->unlock();
00193 }
00194 }
00195 }