$search
00001 00032 #include <unistd.h> 00033 00034 #include <ros/ros.h> 00035 #include <geometry_msgs/Twist.h> 00036 #include <joy/Joy.h> 00037 00038 #include <roslib/Time.h> 00039 00040 00041 #include <stdio.h> 00042 #include <stdlib.h> 00043 00044 #include <pthread.h> 00045 #include <errno.h> 00046 #include <math.h> 00047 00048 #include <cwiid.h> 00049 00050 00051 class WiiMote; 00052 //WiiMote *instance; 00053 00054 00055 class WiiMote { 00056 00057 public: 00058 WiiMote(); 00059 ~WiiMote(); 00060 int init(int retries=5); 00061 void getraw(double *acc, int *buttons, double *nunchuk_stick, 00062 double *nunchuk_acc, int *nunchuk_buttons); 00063 int getdata(double *x, double *y, double *a, int *deadkey); 00064 void shutdown(); 00065 00066 private: 00067 void cwiid_callback(cwiid_wiimote_t *wiimote, int mesg_count, 00068 union cwiid_mesg mesg[], struct timespec *timestamp); 00069 static void cwiid_callback_s(cwiid_wiimote_t *wiimote, int mesg_count, 00070 union cwiid_mesg mesg[], struct timespec *timestamp); 00071 00072 cwiid_wiimote_t *wiimote_; // Wiimote handle 00073 struct acc_cal calib_; // Wiimote calibration data 00074 00075 pthread_cond_t cond_; 00076 pthread_mutex_t mutex_; 00077 00078 int deadkey_; 00079 double x_, y_, a_, a_last_; 00080 bool fresh_, ok_; 00081 00082 // rotation specific parameters 00083 const static double dead_ = 0.2; // size of deadzone [rad] 00084 const static double gain_ = 0.8; // gain outside deadzone 00085 const static double iir_gain_ = 0.3; // gain of iir filter 00086 00087 // if this timeout elapses without any message from the wiimote, 00088 // we assume connection loss 00089 const static int message_timeout_=1; 00090 00091 WiiMote* last_instance_; 00092 static WiiMote* instance_; 00093 }; 00094 00095 WiiMote* WiiMote::instance_ = 0; 00096 00097 00098 WiiMote::WiiMote() 00099 { 00100 x_=0.0; y_=0.0; a_=0.0; deadkey_=0; 00101 00102 pthread_cond_t c = PTHREAD_COND_INITIALIZER; 00103 cond_ = c; 00104 00105 pthread_mutex_t m = PTHREAD_MUTEX_INITIALIZER; 00106 mutex_ = m; 00107 00108 fresh_ = false; 00109 ok_ = true; 00110 00111 last_instance_ = instance_; 00112 instance_ = this; 00113 } 00114 00115 WiiMote::~WiiMote() 00116 { 00117 instance_ = last_instance_; 00118 } 00119 00120 void WiiMote::cwiid_callback_s(cwiid_wiimote_t *wiimote, int mesg_count, 00121 union cwiid_mesg mesg[], struct timespec *timestamp) 00122 { 00123 WiiMote *w; 00124 for(w = instance_; w != 0; w = w->last_instance_) 00125 if(w->wiimote_ == wiimote) 00126 break; 00127 00128 if(w) 00129 w->cwiid_callback(wiimote, mesg_count, mesg, timestamp); 00130 } 00131 00132 00133 void WiiMote::cwiid_callback(cwiid_wiimote_t *wiimote, int mesg_count, 00134 union cwiid_mesg mesg[], struct timespec *timestamp) 00135 { 00136 int i, j, deadkey; 00137 double acc[3], angle, sig, d, l, a, a_iir; 00138 00139 double a_gain = 100 / (gain_ * (M_PI/2.0 - dead_)); /* scale angle values to [-1.0 .. 1.0] */ 00140 00141 for (i = 0; i < mesg_count; i++) { 00142 switch (mesg[i].type) { 00143 00144 case CWIID_MESG_NUNCHUK: 00145 00146 /* convert nunchuck commands to (x, y, a) velocities: The 00147 * joystick is mapped to x and y, the roll (after some 00148 * filtering) is mapped to 'a'. 00149 */ 00150 00151 pthread_mutex_lock(&mutex_); 00152 00153 // Normalize force vectors to [-1 .. 1]. 00154 for (j = 0; j < 3; j++) 00155 acc[j] = ((double) mesg[i].nunchuk_mesg.acc[j] 00156 - calib_.zero[j]) / (calib_.one[j] - calib_.zero[j]); 00157 00158 // Length of force vector 00159 l = sqrt(acc[0]*acc[0] + acc[1]*acc[1] + acc[2]*acc[2]); 00160 00161 angle = atan2(acc[0], acc[2]) / l; // Compute roll angle. 00162 00163 // IIR filter 00164 a_iir = a_last_ * (1 - iir_gain_) + iir_gain_ * angle; 00165 a_last_ = a_iir; // Remember last iir value. 00166 00167 sig = (a_iir < 0) ? -1 : 1; // Do we have positive or negative control input? 00168 d = (fabs(a_iir) > dead_) ? 1 : 0; // Are we inside deadzone? 00169 a = d * gain_ * (a_iir - sig * dead_); // Compute deadzone gain. 00170 00171 // Deadman switch 00172 deadkey = (mesg[i].nunchuk_mesg.buttons & CWIID_NUNCHUK_BTN_Z) ? 1 : 0; 00173 00174 // scale to [-1 .. 1] 00175 y_ = 0.01 * deadkey * (-mesg[i].nunchuk_mesg.stick[CWIID_X] + 127); 00176 x_ = 0.01 * deadkey * ( mesg[i].nunchuk_mesg.stick[CWIID_Y] - 135); 00177 a_ = 0.01 * deadkey * (-a*a_gain); 00178 00179 // limit to [-1 .. 1] 00180 x_ = (x_ > 1) ? 1 : (x_ < -1) ? -1 : x_; 00181 y_ = (y_ > 1) ? 1 : (y_ < -1) ? -1 : y_; 00182 a_ = (a_ > 1) ? 1 : (a_ < -1) ? -1 : a_; 00183 deadkey_ = deadkey; 00184 00185 fresh_ = true; 00186 pthread_cond_broadcast(&cond_); 00187 pthread_mutex_unlock(&mutex_); 00188 00189 break; 00190 case CWIID_MESG_ERROR: 00191 ROS_DEBUG("(callback): Got wiimote error message: %d", mesg->error_mesg.error); 00192 ok_ = false; 00193 break; 00194 default: 00195 ROS_DEBUG("(callback): Unknown Report %d", (int) mesg[i].type); 00196 break; 00197 } 00198 } 00199 } 00200 00201 00202 int WiiMote::init(int retries) 00203 { 00204 bdaddr_t bdaddr = {{0, 0, 0, 0, 0, 0}}; //BDADDR_ANY 00205 00206 /* Connect to the wiimote. Retry a few times if it doesn't work. */ 00207 for (int i = 0; i < retries; i++) { 00208 ROS_INFO("Trying to connect, press 1 + 2 on the wiimote..."); 00209 if ((wiimote_ = cwiid_open(&bdaddr, 0)) != NULL) 00210 break; 00211 } 00212 if (!wiimote_) { 00213 ROS_ERROR("Unable to connect to wiimote."); 00214 return 0; 00215 } 00216 ROS_INFO("Connected."); 00217 00218 if (cwiid_set_rpt_mode(wiimote_, CWIID_RPT_BTN | CWIID_RPT_NUNCHUK | CWIID_RPT_ACC)) { 00219 ROS_ERROR("Error setting report mode."); 00220 return 0; 00221 } 00222 00223 cwiid_get_acc_cal(wiimote_, CWIID_EXT_NUNCHUK, &calib_); 00224 00225 cwiid_enable(wiimote_, CWIID_FLAG_MESG_IFC); 00226 if (cwiid_set_mesg_callback(wiimote_, WiiMote::cwiid_callback_s)) { 00227 ROS_ERROR("Unable to set message callback."); 00228 return 0; 00229 } 00230 return 1; 00231 } 00232 00233 00234 void WiiMote::shutdown() 00235 { 00236 if(wiimote_) 00237 if(cwiid_close(wiimote_)) 00238 ROS_ERROR("Error on wiimote disconnect."); 00239 wiimote_ = 0; 00240 } 00241 00242 00243 void WiiMote::getraw(double *acc, int *buttons, 00244 double *nunchuk_stick, double *nunchuk_acc, int *nunchuk_buttons) 00245 { 00246 struct cwiid_state state; 00247 cwiid_get_state(wiimote_, &state); 00248 00249 if(acc) 00250 for(int i=0; i < 3; i++) 00251 acc[i] = state.acc[i]; 00252 if(buttons) 00253 *buttons = state.buttons; 00254 if(nunchuk_stick) 00255 for(int i=0; i < 2; i++) 00256 nunchuk_stick[i] = state.ext.nunchuk.stick[i]; 00257 if(nunchuk_acc) 00258 for(int i=0; i < 3; i++) 00259 nunchuk_acc[i] = state.ext.nunchuk.acc[i]; 00260 if(nunchuk_buttons) 00261 *nunchuk_buttons = state.ext.nunchuk.buttons; 00262 } 00263 00264 00265 int WiiMote::getdata(double *x, double *y, double *a, int *deadkey) 00266 { 00267 struct timespec timeout; 00268 clock_gettime(CLOCK_REALTIME, &timeout); 00269 timeout.tv_sec += message_timeout_; 00270 00271 pthread_mutex_lock(&mutex_); 00272 00273 // wait for fresh wiimote data 00274 while(!fresh_) { 00275 int rc = pthread_cond_timedwait(&cond_, &mutex_, &timeout); 00276 // the error state is only set after 20 seconds, so we use a 00277 // 1-second timeout ourselves. 00278 if(rc == ETIMEDOUT || !ok_) { 00279 // we have an error - give up and return 00280 pthread_mutex_unlock(&mutex_); 00281 return 0; 00282 } 00283 } 00284 00285 // copy the data 00286 if(x) *x = x_; 00287 if(y) *y = y_; 00288 if(a) *a = a_; 00289 if(deadkey) *deadkey = deadkey_; 00290 00291 fresh_ = false; 00292 00293 pthread_mutex_unlock(&mutex_); 00294 return 1; 00295 } 00296 00297 00298 int main(int argc, char *argv[]) 00299 { 00300 ros::init(argc, argv, "wii"); 00301 ros::NodeHandle n("~"); 00302 00303 double speed, aspeed; 00304 00305 WiiMote mote; 00306 00307 // get maximum speed (m/s) 00308 n.param("speed", speed, 0.1); 00309 n.param("aspeed", aspeed, 1.3); 00310 00311 if(!mote.init()) 00312 return 0; 00313 00314 ros::Publisher pub = n.advertise<geometry_msgs::Twist>("/cmd_vel", 1); 00315 ros::Publisher pub_j = n.advertise<joy::Joy>("wiimote", 1); 00316 00317 ros::Time t = ros::Time::now(); 00318 00319 int zero=0; // count the number of released-deadkey readings 00320 while(n.ok()) { 00321 int d; 00322 double x, y, a; 00323 if(!mote.getdata(&x, &y, &a, &d)) { 00324 ROS_ERROR("Wiimote connection problems, exiting..."); 00325 mote.shutdown(); 00326 return -1; 00327 } 00328 00329 if(d != 0 || zero < 3) { 00330 geometry_msgs::Twist msg; 00331 msg.linear.x = speed*x; 00332 msg.linear.y = speed*y; 00333 msg.angular.z = speed*a*aspeed; 00334 pub.publish(msg); 00335 00336 zero = (d==0) ? zero+1 : 0; 00337 } 00338 00339 // publish raw wiimote data 00340 { 00341 int btn, nbtn; 00342 double acc[3], nacc[3], nstick[2]; 00343 mote.getraw(acc, &btn, nstick, nacc, &nbtn); 00344 00345 joy::Joy msg; 00346 msg.axes.resize(8); 00347 msg.buttons.resize(15); 00348 00349 msg.axes[0] = acc[0]; 00350 msg.axes[1] = acc[1]; 00351 msg.axes[2] = acc[2]; 00352 msg.axes[3] = nacc[0]; 00353 msg.axes[4] = nacc[1]; 00354 msg.axes[5] = nacc[2]; 00355 msg.axes[6] = nstick[0]; 00356 msg.axes[7] = nstick[1]; 00357 00358 for(int i=0; i < 13; i++) 00359 msg.buttons[i] = (btn & (1 << i)) >> i; 00360 00361 msg.buttons[13] = nbtn & 1; 00362 msg.buttons[14] = (nbtn & 2) >> 1; 00363 00364 pub_j.publish(msg); 00365 } 00366 00367 00368 if(ros::Time::now() - t > ros::Duration(1.0)) { 00369 // reread speed parameters 00370 n.getParam("speed", speed); 00371 n.getParam("aspeed", aspeed); 00372 t = ros::Time::now(); 00373 } 00374 } 00375 00376 mote.shutdown(); 00377 00378 return 0; 00379 }