teleop_wii.cc
Go to the documentation of this file.
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 }


teleop_wii
Author(s): Ingo Kresse
autogenerated on Mon Oct 6 2014 08:42:47