vrpn 07.35
Virtual Reality Peripheral Network
 
Loading...
Searching...
No Matches
vrpn_Tracker_ThalmicLabsMyo.C
Go to the documentation of this file.
2
3#ifdef VRPN_INCLUDE_THALMICLABSMYO
4
5#include "vrpn_Connection.h" // for vrpn_CONNECTION_LOW_LATENCY, etc
6#include "quat.h"
7
8/*
9 * TODO :
10 * Use myo timestamp when possible
11 * Timestamps are 64 bit unsigned integers that correspond to a number of microseconds since some (unspecified) period in time. Timestamps are monotonically non-decreasing.
12 * Stream raw emg data ?
13*/
14
15// --------------------------------------------------------------------------
16// Constructor:
17// name (i): device name
18// c (i): vrp8n_Connection
19// useLock (b): require unlock gesture before detecting any gesture
20// armSide (ARMSIDE) : specify (or don't) the arm side for the myo
21
22vrpn_Tracker_ThalmicLabsMyo::vrpn_Tracker_ThalmicLabsMyo(const char *name, vrpn_Connection *c, bool useLock, ARMSIDE armSide) :
23 vrpn_Tracker(name, c),
24 vrpn_Analog(name, c),
25 vrpn_Button_Filter(name, c),
26 _armSide(armSide),
27 hub(NULL),
28 _myo(NULL),
29 _analogChanged(false)
30{
31 while (hub == NULL)
32 {
33 try
34 {
35 hub = new myo::Hub();
36 hub->setLockingPolicy(useLock ? myo::Hub::lockingPolicyStandard : myo::Hub::lockingPolicyNone);
37 hub->addListener(this);
38
40 vrpn_Analog::num_channel = 9; // 0,1,2 : euler rotation - 3,4,5 : acceleration - 6,7,8 : gyroscope
41 vrpn_Button::num_buttons = libmyo_num_poses + 1; // we add one for lock/unlock
42 vrpn_Tracker::num_sensors = 1; // we have orientation and acceleration for the tracker
43
44 vrpn_gettimeofday(&_timestamp, NULL);
45 }
46 catch (const std::exception& e) {
47 std::cerr << "Error: " << e.what() << std::endl;
48 }
49 }
50}
51
52vrpn_Tracker_ThalmicLabsMyo::~vrpn_Tracker_ThalmicLabsMyo()
53{
54 if (hub != NULL) {
55 try {
56 delete hub;
57 } catch (...) {
58 fprintf(stderr, "vrpn_Tracker_ThalmicLabsMyo::~vrpn_Tracker_ThalmicLabsMyo(): delete failed\n");
59 return;
60 }
61 }
62}
63
64void vrpn_Tracker_ThalmicLabsMyo::mainloop() {
65
66 if (hub == NULL)
67 return;
68
69 // server update. We only need to call this once for all three
70 // base devices because it is in the unique base class.
71 server_mainloop();
72
73 hub->runOnce(1);
74
75 // only report once per loop for analog changes
76 if (_analogChanged)
77 {
79 _analogChanged = false;
80 }
81}
82
83void vrpn_Tracker_ThalmicLabsMyo::_report_lock() {
84 vrpn_gettimeofday(&_timestamp, NULL);
86 vrpn_Button::timestamp = _timestamp;
87 buttons[0] = _locked;
89}
90
91void vrpn_Tracker_ThalmicLabsMyo::onPair(myo::Myo* myo, uint64_t timestamp, myo::FirmwareVersion firmwareVersion)
92{
93 std::cout << "Paired with Myo." << std::endl;
94 myo->vibrate(myo->vibrationShort);
95}
96
97void vrpn_Tracker_ThalmicLabsMyo::onConnect(myo::Myo* myo, uint64_t timestamp, myo::FirmwareVersion firmwareVersion)
98{
99 std::cout << "Myo Connected." << std::endl;
100 myo->vibrate(myo->vibrationShort);
101}
102
103void vrpn_Tracker_ThalmicLabsMyo::onDisconnect(myo::Myo* myo, uint64_t timestamp)
104{
105 std::cout << "Myo disconnected." << std::endl;
106 if (myo == _myo)
107 _myo = NULL;
108}
109
110void vrpn_Tracker_ThalmicLabsMyo::onPose(myo::Myo* myo, uint64_t timestamp, myo::Pose pose)
111{
112 if (myo != _myo)
113 return;
114 vrpn_gettimeofday(&_timestamp, NULL);
116 vrpn_Button::timestamp = _timestamp;
117
118
119 // std::cout << "Myo switched to pose " << pose.toString() << "." << std::endl;
120
121 buttons[0] = _locked;
122 // reset all buttons to 0. Maybe we should only do this if rest is on ?
123 for (int i = 1; i < libmyo_num_poses + 1; ++i)
124 buttons[i] = 0;
125 buttons[pose.type() + 1] = 1;
126
128}
129
130void vrpn_Tracker_ThalmicLabsMyo::onArmSync(myo::Myo* myo, uint64_t timestamp, myo::Arm arm, myo::XDirection xDirection)
131{
132 std::cout << "Myo on arm : " << (arm == myo::armLeft ? "Left." : "Right.") << std::endl;
133 if (_armSide == ARMSIDE::ANY || (_armSide == ARMSIDE::RIGHT && arm == myo::armRight) || (_armSide == ARMSIDE::LEFT && arm == myo::armLeft))
134 {
135 std::cout << "Myo : " << d_servicename << " accepted" << std::endl;
136 myo->vibrate(myo->vibrationShort);
137 _myo = myo;
138 }
139 else
140 {
141 std::cout << "Myo : " << d_servicename << " NOT accepted : wrong arm detected" << std::endl;
142 }
143
144}
145
146void vrpn_Tracker_ThalmicLabsMyo::onArmUnsync(myo::Myo* myo, uint64_t timestamp)
147{
148 if (myo != _myo)
149 return;
150 std::cout << "Myo : " << d_servicename <<" removed from arm."<< std::endl;
151 myo->vibrate(myo->vibrationLong);
152 _myo = NULL;
153}
154
155void vrpn_Tracker_ThalmicLabsMyo::onUnlock(myo::Myo* myo, uint64_t timestamp)
156{
157 if (myo != _myo)
158 return;
159 _locked = false;
160 _report_lock();
161}
162
163void vrpn_Tracker_ThalmicLabsMyo::onLock(myo::Myo* myo, uint64_t timestamp)
164{
165 if (myo != _myo)
166 return;
167 _locked = true;
168 _report_lock();
169}
170
171void vrpn_Tracker_ThalmicLabsMyo::onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& rotation)
172{
173 if (myo != _myo)
174 return;
175 if (!d_connection) {
176 return;
177 }
178 vrpn_gettimeofday(&_timestamp, NULL);
180 vrpn_Tracker::timestamp = _timestamp;
181 vrpn_Analog::timestamp = _timestamp;
182
183 d_quat[0] = rotation.x();
184 d_quat[1] = rotation.y();
185 d_quat[2] = rotation.z();
186 d_quat[3] = rotation.w();
187
188 // do the same as analog, with euler angles (maybe offset from when OnArmSync?)
189 q_vec_type euler;
190
191 q_to_euler(euler, d_quat);
192 channel[ANALOG_ROTATION_X] = euler[Q_ROLL];
193 channel[ANALOG_ROTATION_Y] = euler[Q_PITCH];
194 channel[ANALOG_ROTATION_Z] = euler[Q_YAW];
195
196
197 char msgbuf[1000];
198 int len = vrpn_Tracker::encode_to(msgbuf);
199 if (d_connection->pack_message(len, _timestamp, position_m_id, d_sender_id, msgbuf, vrpn_CONNECTION_LOW_LATENCY)) {
200 fprintf(stderr, "Thalmic Lab's myo tracker: can't write message: tossing\n");
201 }
202
203 _analogChanged = true;
204}
205
206void vrpn_Tracker_ThalmicLabsMyo::onAccelerometerData(myo::Myo* myo, uint64_t timestamp, const myo::Vector3<float>& accel)
207{
208 if (myo != _myo)
209 return;
210 if (!d_connection) {
211 return;
212 }
213 vrpn_gettimeofday(&_timestamp, NULL);
215 vrpn_Tracker::timestamp = _timestamp;
216 vrpn_Analog::timestamp = _timestamp;
217
218 acc[0] = accel[0];
219 acc[1] = accel[1];
220 acc[2] = accel[2];
221
222 // same for analog
223 channel[ANALOG_ACCEL_X] = accel[0];
224 channel[ANALOG_ACCEL_Y] = accel[1];
225 channel[ANALOG_ACCEL_Z] = accel[2];
226
227 char msgbuf[1000];
228 int len = encode_acc_to(msgbuf);
229 if (d_connection->pack_message(len, _timestamp, accel_m_id, d_sender_id, msgbuf, vrpn_CONNECTION_LOW_LATENCY)) {
230 fprintf(stderr, "Thalmic Lab's myo tracker: can't write message: tossing\n");
231 }
232 _analogChanged = true;
233}
234
235void vrpn_Tracker_ThalmicLabsMyo::onGyroscopeData(myo::Myo* myo, uint64_t timestamp, const myo::Vector3<float>& gyro)
236{
237 if (myo != _myo)
238 return;
239 vrpn_gettimeofday(&_timestamp, NULL);
241
242 vrpn_Analog::timestamp = _timestamp;
243
244 channel[ANALOG_GYRO_X] = gyro[0];
245 channel[ANALOG_GYRO_Y] = gyro[1];
246 channel[ANALOG_GYRO_Z] = gyro[2];
247
248 _analogChanged = true;
249}
250
251#endif
252
struct timeval timestamp
Definition vrpn_Analog.h:41
vrpn_int32 num_channel
Definition vrpn_Analog.h:40
virtual void report_changes(vrpn_uint32 class_of_service=vrpn_CONNECTION_LOW_LATENCY, const struct timeval time=vrpn_ANALOG_NOW)
Send a report only if something has changed (for servers) Optionally, tell what time to stamp the val...
Definition vrpn_Analog.C:71
All button servers should derive from this class, which provides the ability to turn any of the butto...
Definition vrpn_Button.h:66
vrpn_int32 num_buttons
Definition vrpn_Button.h:48
struct timeval timestamp
Definition vrpn_Button.h:49
virtual void report_changes(void)
Generic connection class not specific to the transport mechanism.
virtual int encode_to(char *buf)
vrpn_int32 num_sensors
struct timeval timestamp
const vrpn_uint32 vrpn_CONNECTION_LOW_LATENCY
double vrpn_TimevalDurationSeconds(struct timeval endT, struct timeval startT)
Return the number of seconds between startT and endT as a floating-point value.
#define vrpn_gettimeofday
Definition vrpn_Shared.h:99