teleop_wiimote.cpp
Go to the documentation of this file.
00001 /*
00002  * ROS Node for using a wiimote control unit to direct a robot.
00003  * Copyright (c) 2016, Intel Corporation.
00004  *
00005  * This program is free software; you can redistribute it and/or modify it
00006  * under the terms and conditions of the GNU General Public License,
00007  * version 2, as published by the Free Software Foundation.
00008  *
00009  * This program is distributed in the hope it will be useful, but WITHOUT
00010  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
00011  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
00012  * more details.
00013  */
00014 
00015 /*
00016  * Initial C++ implementation by
00017  *   Mark Horn <mark.d.horn@intel.com>
00018  *
00019  * Revisions:
00020  *
00021  */
00022 
00023 #include "wiimote/teleop_wiimote.h"
00024 #include "geometry_msgs/Twist.h"
00025 #include "sensor_msgs/JoyFeedbackArray.h"
00026 
00027 #include <string>
00028 
00029 TeleopWiimote::TeleopWiimote()
00030 {
00031   ros::NodeHandle nh_private("~");
00032   ros::NodeHandle nh;
00033   std::string base_name;
00034 
00035   if (nh_private.hasParam("base"))
00036   {
00037     if (nh_private.getParam("base", base_name))
00038     {
00039       ROS_INFO("User namespace '%s' for robot limits.", base_name.c_str());
00040     }
00041   }
00042   base_name += "/";
00043 
00044   if (!nh.getParam(base_name + "linear/x/max_velocity", linear_x_max_velocity_))
00045   {
00046     linear_x_max_velocity_ = DEFAULT_MAX_LINEAR_X;
00047     ROS_INFO("Defaulting to maximum linear x: %f", linear_x_max_velocity_);
00048   }
00049   else
00050   {
00051     ROS_INFO("Maximum linear x: %f", linear_x_max_velocity_);
00052   }
00053   if (!nh.getParam(base_name + "linear/x/min_velocity", linear_x_min_velocity_))
00054   {
00055     linear_x_min_velocity_ = -1.0 * DEFAULT_MAX_LINEAR_X;
00056     ROS_INFO("Defaulting to minimum linear x: %f", linear_x_min_velocity_);
00057   }
00058   else
00059   {
00060     ROS_INFO("Minimum linear x: %f", linear_x_min_velocity_);
00061   }
00062 
00063   if (!nh.getParam(base_name + "angular/z/max_velocity", angular_z_max_velocity_))
00064   {
00065     angular_z_max_velocity_ = DEFAULT_MAX_ANGULAR_Z;
00066     ROS_INFO("Defaulting to maximum angular z: %f", angular_z_max_velocity_);
00067   }
00068   else
00069   {
00070     ROS_INFO("Maximum angular z: %f", angular_z_max_velocity_);
00071   }
00072   if (!nh.getParam(base_name + "angular/z/min_velocity", angular_z_min_velocity_))
00073   {
00074     angular_z_min_velocity_ = -1.0 * DEFAULT_MAX_ANGULAR_Z;
00075     ROS_INFO("Defaulting to minimum angular z: %f", angular_z_min_velocity_);
00076   }
00077   else
00078   {
00079     ROS_INFO("Minimum angular z: %f", angular_z_min_velocity_);
00080   }
00081 
00082   // Percent throttle limiter
00083   if (nh_private.hasParam("linear/x/throttle_percent"))
00084   {
00085     if (!nh_private.getParam("linear/x/throttle_percent", percent_linear_throttle_))
00086     {
00087       ROS_WARN("Failed to get linear x throttle percent; using %3.0f", DEFAULT_PERCENT_LINEAR_THROTTLE * 100.0);
00088       percent_linear_throttle_ = DEFAULT_PERCENT_LINEAR_THROTTLE;
00089     }
00090   }
00091   else
00092   {
00093     percent_linear_throttle_ = DEFAULT_PERCENT_LINEAR_THROTTLE;
00094   }
00095   nh_private.setParam("linear/x/throttle_percent", percent_linear_throttle_);
00096   ROS_INFO("Linear X Throttle Percent: %3.0f", percent_linear_throttle_ * 100.0);
00097 
00098   if (nh_private.hasParam("angular/z/throttle_percent"))
00099   {
00100     if (!nh_private.getParam("angular/z/throttle_percent", percent_angular_throttle_))
00101     {
00102       ROS_WARN("Failed to get angular z throttle percent; using %3.0f", DEFAULT_PERCENT_ANGULAR_THROTTLE * 100.0);
00103       percent_angular_throttle_ = DEFAULT_PERCENT_ANGULAR_THROTTLE;
00104     }
00105   }
00106   else
00107   {
00108     percent_angular_throttle_ = DEFAULT_PERCENT_ANGULAR_THROTTLE;
00109   }
00110   nh_private.setParam("angular/x/throttle_percent", percent_angular_throttle_);
00111   ROS_INFO("Angular Z Throttle Percent: %3.0f", percent_angular_throttle_ * 100.0);
00112 
00113   vel_pub_ = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
00114   joy_pub_ = nh.advertise<sensor_msgs::JoyFeedbackArray>("joy/set_feedback", 1);
00115 
00116   joy_sub_ = nh.subscribe<sensor_msgs::Joy>("wiimote/nunchuk", 10, &TeleopWiimote::joyCallback, this);
00117   wiimote_sub_ = nh.subscribe<wiimote::State>("wiimote/state", 10, &TeleopWiimote::wiimoteStateCallback, this);
00118 
00119   dpad_in_use_ = false;
00120   njoy_in_use_ = false;
00121 }
00122 
00123 void TeleopWiimote::setLEDFeedback(double value)
00124 {
00125   sensor_msgs::JoyFeedbackArray joy_feedback_array;
00126   sensor_msgs::JoyFeedback fb_led0;
00127   sensor_msgs::JoyFeedback fb_led1;
00128   sensor_msgs::JoyFeedback fb_led2;
00129   sensor_msgs::JoyFeedback fb_led3;
00130 
00131   fb_led0.type = sensor_msgs::JoyFeedback::TYPE_LED;
00132   fb_led0.id = 0;
00133   fb_led0.intensity = 0.0;
00134   fb_led1.type = sensor_msgs::JoyFeedback::TYPE_LED;
00135   fb_led1.id = 1;
00136   fb_led1.intensity = 0.0;
00137   fb_led2.type = sensor_msgs::JoyFeedback::TYPE_LED;
00138   fb_led2.id = 2;
00139   fb_led2.intensity = 0.0;
00140   fb_led3.type = sensor_msgs::JoyFeedback::TYPE_LED;
00141   fb_led3.id = 3;
00142   fb_led3.intensity = 0.0;
00143 
00144   if (value > 10.0)
00145   {
00146     fb_led0.intensity = 1.0;
00147   }
00148   if (value > 35.0)
00149   {
00150     fb_led1.intensity = 1.0;
00151   }
00152   if (value > 60.0)
00153   {
00154     fb_led2.intensity = 1.0;
00155   }
00156   if (value > 85.0)
00157   {
00158     fb_led3.intensity = 1.0;
00159   }
00160 
00161   joy_feedback_array.array.push_back(fb_led0);
00162   joy_feedback_array.array.push_back(fb_led1);
00163   joy_feedback_array.array.push_back(fb_led2);
00164   joy_feedback_array.array.push_back(fb_led3);
00165 
00166   joy_pub_.publish(joy_feedback_array);
00167 }
00168 
00169 void TeleopWiimote::rumbleFeedback(int useconds)
00170 {
00171   sensor_msgs::JoyFeedbackArray joy_feedback_array;
00172   sensor_msgs::JoyFeedback fb_rumble;
00173 
00174   fb_rumble.type = sensor_msgs::JoyFeedback::TYPE_RUMBLE;
00175   fb_rumble.id = 0;
00176 
00177   fb_rumble.intensity = 1.0;
00178 
00179   joy_feedback_array.array.push_back(fb_rumble);
00180 
00181   joy_pub_.publish(joy_feedback_array);
00182   usleep(useconds);
00183   fb_rumble.intensity = 0.0;
00184 
00185   joy_feedback_array.array.push_back(fb_rumble);
00186 
00187   joy_pub_.publish(joy_feedback_array);
00188 }
00189 
00190 void TeleopWiimote::joyCallback(const sensor_msgs::Joy::ConstPtr& joy)
00191 {
00192   geometry_msgs::Twist vel;
00193 
00194   static const int MSG_BTN_Z     = 0;
00195   static const int MSG_BTN_C     = 1;
00196 
00197   if (dpad_in_use_)
00198   {
00199     return;
00200   }
00201 
00202   float x = joy->axes[0];
00203   float y = joy->axes[1];
00204   float const abs_error = 0.000001;
00205 
00206   if (fabs(x) > abs_error || fabs(y) > abs_error)
00207   {
00208     njoy_in_use_ = true;
00209 
00210     float boost = 1.0;
00211 
00212     ROS_INFO("nunchuk: x: %f, y: %f", x, y);
00213 
00214     if (joy->buttons[MSG_BTN_Z] ||
00215         joy->buttons[MSG_BTN_C])
00216     {
00217       ROS_INFO("buttons[]: Z: %d, C: %d",
00218           joy->buttons[MSG_BTN_Z],
00219           joy->buttons[MSG_BTN_C]);
00220 
00221       // Z-Button is thrusters on!
00222       if (joy->buttons[MSG_BTN_Z])
00223       {
00224         boost = 2.0;
00225       }
00226 
00227       // C-Button is easy does it.
00228       if (joy->buttons[MSG_BTN_C])
00229       {
00230         boost = 0.25;
00231       }
00232     }
00233 
00234     if (y >= 0.0)
00235     {
00236       vel.linear.x = fmin((y * boost * (linear_x_max_velocity_ * percent_linear_throttle_)),
00237           linear_x_max_velocity_);
00238 
00239       if (x >= 0.0)
00240       {
00241         vel.angular.z = fmin((x * boost * (angular_z_max_velocity_ * percent_angular_throttle_)),
00242             angular_z_max_velocity_);
00243       }
00244       else
00245       {
00246         vel.angular.z = fmax((fabs(x) * boost * (angular_z_min_velocity_ * percent_angular_throttle_)),
00247             angular_z_min_velocity_);
00248       }
00249     }
00250     else
00251     {
00252       vel.linear.x = fmax((fabs(y) * boost * (linear_x_min_velocity_ * percent_linear_throttle_)),
00253           linear_x_min_velocity_);
00254 
00255       if (x > 0.0)
00256       {
00257         vel.angular.z = fmax((x * boost * (angular_z_min_velocity_ * percent_angular_throttle_)),
00258             angular_z_min_velocity_);
00259       }
00260       else
00261       {
00262         vel.angular.z = fmin((fabs(x) * boost * (angular_z_max_velocity_ * percent_angular_throttle_)),
00263             angular_z_max_velocity_);
00264       }
00265     }
00266 
00267     // In order to spin-in-place left or right, we need full angular with NO linear component.
00268     // To enable this, we will publish no linear motion if nunchuk joystick Y value is
00269     // "really small" as the joy stick isn't 100 accurate.
00270     if (fabs(y) < 0.01)
00271     {
00272       vel.linear.x = 0;
00273     }
00274 
00275     vel_pub_.publish(vel);
00276   }
00277   else
00278   {
00279     if (njoy_in_use_)
00280     {
00281       vel_pub_.publish(vel);
00282 
00283       njoy_in_use_ = false;
00284     }
00285   }
00286 }
00287 void TeleopWiimote::wiimoteStateCallback(const wiimote::State::ConstPtr& wiistate)
00288 {
00289   ros::NodeHandle nh_private("~");
00290   geometry_msgs::Twist vel;
00291 
00292   static const int MSG_BTN_1     = 0;
00293   static const int MSG_BTN_2     = 1;
00294   static const int MSG_BTN_PLUS  = 2;
00295   static const int MSG_BTN_MINUS = 3;
00296   static const int MSG_BTN_A     = 4;
00297   static const int MSG_BTN_B     = 5;
00298   static const int MSG_BTN_UP    = 6;
00299   static const int MSG_BTN_DOWN  = 7;
00300   static const int MSG_BTN_LEFT  = 8;
00301   static const int MSG_BTN_RIGHT = 9;
00302   static const int MSG_BTN_HOME  = 10;
00303 
00304   static bool one_depressed = false;
00305   static bool two_depressed = false;
00306   static bool plus_depressed = false;
00307   static bool minus_depressed = false;
00308   static bool home_depressed = false;
00309 
00310 
00311   // 1-Button used to set the amount of Linear Throttle
00312   // Pressing the button show approx setting level on the
00313   // Wiimote LEDs (see setLEDFeedback for levels).
00314   // Wiimote uses a short Rumble when the minimum or
00315   // maximum is reached.
00316   // +-Button increases; --Button decreases while hold 1-Button
00317   if (wiistate->buttons[MSG_BTN_1])
00318   {
00319     if (wiistate->buttons[MSG_BTN_PLUS])
00320     {
00321       if (!plus_depressed)
00322       {
00323         percent_linear_throttle_ += 0.05;
00324         if (percent_linear_throttle_ >= 1.0)
00325         {
00326           rumbleFeedback(100000);
00327         }
00328         percent_linear_throttle_ = fmin(percent_linear_throttle_, 1.0);
00329         plus_depressed = true;
00330 
00331         setLEDFeedback(percent_linear_throttle_ * 100.0);
00332 
00333         nh_private.setParam("linear/x/throttle_percent", percent_linear_throttle_);
00334         ROS_INFO("Linear X Throttle Percent: %3.0f", percent_linear_throttle_ * 100.0);
00335       }
00336     }
00337     else
00338     {
00339       plus_depressed = false;
00340 
00341       if (wiistate->buttons[MSG_BTN_MINUS])
00342       {
00343         if (!minus_depressed)
00344         {
00345           percent_linear_throttle_ -= 0.05;
00346           if (percent_linear_throttle_ <= 0.1)
00347           {
00348             rumbleFeedback(100000);
00349           }
00350           percent_linear_throttle_ = fmax(percent_linear_throttle_, 0.1);
00351           minus_depressed = true;
00352 
00353           setLEDFeedback(percent_linear_throttle_ * 100.0);
00354 
00355           nh_private.setParam("linear/x/throttle_percent", percent_linear_throttle_);
00356           ROS_INFO("Linear X Throttle Percent: %3.0f", percent_linear_throttle_ * 100.0);
00357         }
00358       }
00359       else
00360       {
00361         minus_depressed = false;
00362       }
00363     }
00364 
00365     if (!one_depressed)
00366     {
00367       setLEDFeedback(percent_linear_throttle_ * 100.0);
00368 
00369       nh_private.setParam("linear/x/throttle_percent", percent_linear_throttle_);
00370       ROS_INFO("Linear X Throttle Percent: %3.0f", percent_linear_throttle_ * 100.0);
00371 
00372       one_depressed = true;
00373     }
00374   }
00375   // 2-Button used to set the amount of Angular Throttle
00376   // Same function and feedbacks as 1-Button (see above)
00377   else if (wiistate->buttons[MSG_BTN_2])
00378   {
00379     if (wiistate->buttons[MSG_BTN_PLUS])
00380     {
00381       if (!plus_depressed)
00382       {
00383         percent_angular_throttle_ += 0.05;
00384         if (percent_angular_throttle_ >= 1.0)
00385         {
00386           rumbleFeedback(100000);
00387         }
00388         percent_angular_throttle_ = fmin(percent_angular_throttle_, 1.0);
00389         plus_depressed = true;
00390 
00391         setLEDFeedback(percent_angular_throttle_ * 100.0);
00392 
00393         nh_private.setParam("angular/x/throttle_percent", percent_angular_throttle_);
00394         ROS_INFO("Angular Z Throttle Percent: %3.0f", percent_angular_throttle_ * 100.0);
00395       }
00396     }
00397     else
00398     {
00399       plus_depressed = false;
00400 
00401       if (wiistate->buttons[MSG_BTN_MINUS])
00402       {
00403         if (!minus_depressed)
00404         {
00405           percent_angular_throttle_ -= 0.05;
00406           if (percent_angular_throttle_ <= 0.1)
00407           {
00408             rumbleFeedback(100000);
00409           }
00410           percent_angular_throttle_ = fmax(percent_angular_throttle_, 0.1);
00411           minus_depressed = true;
00412 
00413           setLEDFeedback(percent_angular_throttle_ * 100.0);
00414 
00415           nh_private.setParam("angular/x/throttle_percent", percent_angular_throttle_);
00416           ROS_INFO("Angular Z Throttle Percent: %3.0f", percent_angular_throttle_ * 100.0);
00417         }
00418       }
00419       else
00420       {
00421         minus_depressed = false;
00422       }
00423     }
00424 
00425     if (!two_depressed)
00426     {
00427       setLEDFeedback(percent_angular_throttle_ * 100.0);
00428 
00429       nh_private.setParam("angular/x/throttle_percent", percent_angular_throttle_);
00430       ROS_INFO("Angular Z Throttle Percent: %3.0f", percent_angular_throttle_ * 100.0);
00431 
00432       two_depressed = true;
00433     }
00434   }
00435   else
00436   {
00437     if (one_depressed || two_depressed)
00438     {
00439       setLEDFeedback(0.0);
00440     }
00441 
00442     one_depressed = false;
00443     two_depressed = false;
00444 
00445     // Home-Button used the Wiimote LEDs (see setLEDFeedback for levels).
00446     // to show the approx battery leve of the Wiimote.
00447     // Only works if the 1-Button or 2-Button are not in use.
00448     if (wiistate->buttons[MSG_BTN_HOME])
00449     {
00450       if (!home_depressed)
00451       {
00452         ROS_INFO("Battery[]: raw: %f, percent: %f", wiistate->raw_battery, wiistate->percent_battery);
00453         setLEDFeedback(wiistate->percent_battery);
00454         home_depressed = true;
00455       }
00456     }
00457     else
00458     {
00459       if (home_depressed)
00460       {
00461         setLEDFeedback(0.0);
00462       }
00463 
00464       home_depressed = false;
00465     }
00466   }
00467 
00468   if (!njoy_in_use_ &&
00469       (wiistate->buttons[MSG_BTN_RIGHT] ||
00470        wiistate->buttons[MSG_BTN_LEFT] ||
00471        wiistate->buttons[MSG_BTN_UP] ||
00472        wiistate->buttons[MSG_BTN_DOWN]))
00473   {
00474     dpad_in_use_ = true;
00475 
00476     ROS_INFO("buttons[]: Right: %d, Left: %d, Up: %d, Down: %d, A: %d, B: %d",
00477         wiistate->buttons[MSG_BTN_RIGHT],
00478         wiistate->buttons[MSG_BTN_LEFT],
00479         wiistate->buttons[MSG_BTN_UP],
00480         wiistate->buttons[MSG_BTN_DOWN],
00481         wiistate->buttons[MSG_BTN_A],
00482         wiistate->buttons[MSG_BTN_B]);
00483 
00484     float boost = 1.0;
00485 
00486     // B-Button is thrusters on!
00487     if (wiistate->buttons[MSG_BTN_B])
00488     {
00489       boost = 2.0;
00490     }
00491 
00492     // A-Button is easy does it.
00493     if (wiistate->buttons[MSG_BTN_A])
00494     {
00495       boost = 0.25;
00496     }
00497 
00498     if (wiistate->buttons[MSG_BTN_UP])
00499     {
00500       vel.linear.x = fmin((boost * (linear_x_max_velocity_ * percent_linear_throttle_)),
00501           linear_x_max_velocity_);
00502     }
00503     else if (wiistate->buttons[MSG_BTN_DOWN])
00504     {
00505       vel.linear.x = fmax((boost * (linear_x_min_velocity_ * percent_linear_throttle_)),
00506           linear_x_min_velocity_);
00507     }
00508 
00509     if (wiistate->buttons[MSG_BTN_LEFT])
00510     {
00511       vel.angular.z = fmin((boost * (angular_z_max_velocity_ * percent_angular_throttle_)),
00512           angular_z_max_velocity_);
00513     }
00514     else if (wiistate->buttons[MSG_BTN_RIGHT])
00515     {
00516       vel.angular.z = fmax((boost * (angular_z_min_velocity_ * percent_angular_throttle_)),
00517           angular_z_min_velocity_);
00518     }
00519 
00520     vel_pub_.publish(vel);
00521   }
00522   else
00523   {
00524     if (dpad_in_use_)
00525     {
00526       vel_pub_.publish(vel);
00527       dpad_in_use_ = false;
00528     }
00529   }
00530 }
00531 
00532 int main(int argc, char** argv)
00533 {
00534   ros::init(argc, argv, "teleop_wiimote");
00535   TeleopWiimote teleop_wiimote;
00536 
00537   ros::spin();
00538 }


wiimote
Author(s): Andreas Paepcke, Melonee Wise, Mark Horn
autogenerated on Sat Jun 8 2019 20:54:42