joy_node.cpp
Go to the documentation of this file.
00001 /*
00002  * teleop_pr2
00003  * Copyright (c) 2009, Willow Garage, Inc.
00004  * All rights reserved.
00005  * 
00006  * Redistribution and use in source and binary forms, with or without
00007  * modification, are permitted provided that the following conditions are met:
00008  * 
00009  *     * Redistributions of source code must retain the above copyright
00010  *       notice, this list of conditions and the following disclaimer.
00011  *     * Redistributions in binary form must reproduce the above copyright
00012  *       notice, this list of conditions and the following disclaimer in the
00013  *       documentation and/or other materials provided with the distribution.
00014  *     * Neither the name of the <ORGANIZATION> nor the names of its
00015  *       contributors may be used to endorse or promote products derived from
00016  *       this software without specific prior written permission.
00017  * 
00018  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00019  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00020  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00021  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00022  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00023  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00024  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00025  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00026  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00027  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00028  * POSSIBILITY OF SUCH DAMAGE.
00029  */
00030 
00031 //\author: Blaise Gassend
00032 
00033 #include <unistd.h>
00034 #include <math.h>
00035 #include <linux/joystick.h>
00036 #include <fcntl.h>
00037 #include <sys/stat.h>
00038 #include <dirent.h>
00039 #include <diagnostic_updater/diagnostic_updater.h>
00040 #include "ros/ros.h"
00041 #include <sensor_msgs/Joy.h>
00042 
00043 
00045 class Joystick
00046 {
00047 private:
00048   ros::NodeHandle nh_;
00049   bool open_;
00050   bool sticky_buttons_;
00051   bool default_trig_val_;
00052   std::string joy_dev_;
00053   std::string joy_dev_name_;
00054   double deadzone_;
00055   double autorepeat_rate_;   // in Hz.  0 for no repeat.
00056   double coalesce_interval_; // Defaults to 100 Hz rate limit.
00057   int event_count_;
00058   int pub_count_;
00059   ros::Publisher pub_;
00060   double lastDiagTime_;
00061   
00062   diagnostic_updater::Updater diagnostic_;
00063   
00065   void diagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat)
00066   {
00067     double now = ros::Time::now().toSec();
00068     double interval = now - lastDiagTime_;
00069     if (open_)
00070       stat.summary(0, "OK");
00071     else
00072       stat.summary(2, "Joystick not open.");
00073     
00074     stat.add("topic", pub_.getTopic());
00075     stat.add("device", joy_dev_);
00076     stat.add("device name", joy_dev_name_);
00077     stat.add("dead zone", deadzone_);
00078     stat.add("autorepeat rate (Hz)", autorepeat_rate_);
00079     stat.add("coalesce interval (s)", coalesce_interval_);
00080     stat.add("recent joystick event rate (Hz)", event_count_ / interval);
00081     stat.add("recent publication rate (Hz)", pub_count_ / interval);
00082     stat.add("subscribers", pub_.getNumSubscribers());
00083     stat.add("default trig val", default_trig_val_);
00084     stat.add("sticky buttons", sticky_buttons_);
00085     event_count_ = 0;
00086     pub_count_ = 0;
00087     lastDiagTime_ = now;
00088   }
00089 
00093   std::string get_dev_by_joy_name(const std::string& joy_name)
00094   {
00095     const char path[] = "/dev/input"; // no trailing / here
00096     struct dirent *entry;
00097     struct stat stat_buf;
00098 
00099     DIR *dev_dir = opendir(path);
00100     if (dev_dir == NULL)
00101     {
00102       ROS_ERROR("Couldn't open %s. Error %i: %s.", path, errno, strerror(errno));
00103       return "";
00104     }
00105 
00106     while ((entry = readdir(dev_dir)) != NULL)
00107     {
00108       // filter entries
00109       if (strncmp(entry->d_name, "js", 2) != 0) // skip device if it's not a joystick
00110         continue;
00111       std::string current_path = std::string(path) + "/" + entry->d_name;
00112       if (stat(current_path.c_str(), &stat_buf) == -1)
00113         continue;
00114       if (!S_ISCHR(stat_buf.st_mode)) // input devices are character devices, skip other
00115         continue;
00116 
00117       // get joystick name
00118       int joy_fd = open(current_path.c_str(), O_RDONLY);
00119       if (joy_fd == -1)
00120         continue;
00121 
00122       char current_joy_name[128];
00123       if (ioctl(joy_fd, JSIOCGNAME(sizeof(current_joy_name)), current_joy_name) < 0)
00124         strncpy(current_joy_name, "Unknown", sizeof(current_joy_name));
00125 
00126       close(joy_fd);
00127 
00128       ROS_INFO("Found joystick: %s (%s).", current_joy_name, current_path.c_str());
00129 
00130       if (strcmp(current_joy_name, joy_name.c_str()) == 0)
00131       {
00132           closedir(dev_dir);
00133           return current_path;
00134       }
00135     }
00136 
00137     closedir(dev_dir);
00138     return "";
00139   }
00140   
00141 public:
00142   Joystick() : nh_(), diagnostic_()
00143   {}
00144   
00146   int main(int argc, char **argv)
00147   {
00148     diagnostic_.add("Joystick Driver Status", this, &Joystick::diagnostics);
00149     diagnostic_.setHardwareID("none");
00150 
00151     // Parameters
00152     ros::NodeHandle nh_param("~");
00153     pub_ = nh_.advertise<sensor_msgs::Joy>("joy", 1);
00154     nh_param.param<std::string>("dev", joy_dev_, "/dev/input/js0");
00155     nh_param.param<std::string>("dev_name", joy_dev_name_, "");
00156     nh_param.param<double>("deadzone", deadzone_, 0.05);
00157     nh_param.param<double>("autorepeat_rate", autorepeat_rate_, 0);
00158     nh_param.param<double>("coalesce_interval", coalesce_interval_, 0.001);
00159     nh_param.param<bool>("default_trig_val",default_trig_val_,false);
00160     nh_param.param<bool>("sticky_buttons", sticky_buttons_, false);
00161 
00162     // Checks on parameters
00163     if (!joy_dev_name_.empty())
00164     {
00165         std::string joy_dev_path = get_dev_by_joy_name(joy_dev_name_);
00166         if (joy_dev_path.empty())
00167             ROS_ERROR("Couldn't find a joystick with name %s. Falling back to default device.", joy_dev_name_.c_str());
00168         else
00169         {
00170             ROS_INFO("Using %s as joystick device.", joy_dev_path.c_str());
00171             joy_dev_ = joy_dev_path;
00172         }
00173     }
00174 
00175     if (autorepeat_rate_ > 1 / coalesce_interval_)
00176       ROS_WARN("joy_node: autorepeat_rate (%f Hz) > 1/coalesce_interval (%f Hz) does not make sense. Timing behavior is not well defined.", autorepeat_rate_, 1/coalesce_interval_);
00177     
00178     if (deadzone_ >= 1)
00179     {
00180       ROS_WARN("joy_node: deadzone greater than 1 was requested. The semantics of deadzone have changed. It is now related to the range [-1:1] instead of [-32767:32767]. For now I am dividing your deadzone by 32767, but this behavior is deprecated so you need to update your launch file.");
00181       deadzone_ /= 32767;
00182     }
00183     
00184     if (deadzone_ > 0.9)
00185     {
00186       ROS_WARN("joy_node: deadzone (%f) greater than 0.9, setting it to 0.9", deadzone_);
00187       deadzone_ = 0.9;
00188     }
00189     
00190     if (deadzone_ < 0)
00191     {
00192       ROS_WARN("joy_node: deadzone_ (%f) less than 0, setting to 0.", deadzone_);
00193       deadzone_ = 0;
00194     }
00195 
00196     if (autorepeat_rate_ < 0)
00197     {
00198       ROS_WARN("joy_node: autorepeat_rate (%f) less than 0, setting to 0.", autorepeat_rate_);
00199       autorepeat_rate_ = 0;
00200     }
00201     
00202     if (coalesce_interval_ < 0)
00203     {
00204       ROS_WARN("joy_node: coalesce_interval (%f) less than 0, setting to 0.", coalesce_interval_);
00205       coalesce_interval_ = 0;
00206     }
00207     
00208     // Parameter conversions
00209     double autorepeat_interval = 1 / autorepeat_rate_;
00210     double scale = -1. / (1. - deadzone_) / 32767.;
00211     double unscaled_deadzone = 32767. * deadzone_;
00212 
00213     js_event event;
00214     struct timeval tv;
00215     fd_set set;
00216     int joy_fd;
00217     event_count_ = 0;
00218     pub_count_ = 0;
00219     lastDiagTime_ = ros::Time::now().toSec();
00220     
00221     // Big while loop opens, publishes
00222     while (nh_.ok())
00223     {                                      
00224       open_ = false;
00225       diagnostic_.force_update();
00226       bool first_fault = true;
00227       while (true)
00228       {
00229         ros::spinOnce();
00230         if (!nh_.ok())
00231           goto cleanup;
00232         joy_fd = open(joy_dev_.c_str(), O_RDONLY);
00233         if (joy_fd != -1)
00234         {
00235           // There seems to be a bug in the driver or something where the
00236           // initial events that are to define the initial state of the
00237           // joystick are not the values of the joystick when it was opened
00238           // but rather the values of the joystick when it was last closed.
00239           // Opening then closing and opening again is a hack to get more
00240           // accurate initial state data.
00241           close(joy_fd);
00242           joy_fd = open(joy_dev_.c_str(), O_RDONLY);
00243         }
00244         if (joy_fd != -1)
00245           break;
00246         if (first_fault)
00247         {
00248           ROS_ERROR("Couldn't open joystick %s. Will retry every second.", joy_dev_.c_str());
00249           first_fault = false;
00250         }
00251         sleep(1.0);
00252         diagnostic_.update();
00253       }
00254       
00255       ROS_INFO("Opened joystick: %s. deadzone_: %f.", joy_dev_.c_str(), deadzone_);
00256       open_ = true;
00257       diagnostic_.force_update();
00258       
00259       bool tv_set = false;
00260       bool publication_pending = false;
00261       tv.tv_sec = 1;
00262       tv.tv_usec = 0;
00263       sensor_msgs::Joy joy_msg; // Here because we want to reset it on device close.
00264       double val; //Temporary variable to hold event values
00265       sensor_msgs::Joy last_published_joy_msg; // used for sticky buttons option
00266       sensor_msgs::Joy sticky_buttons_joy_msg; // used for sticky buttons option
00267       while (nh_.ok()) 
00268       {
00269         ros::spinOnce();
00270         
00271         bool publish_now = false;
00272         bool publish_soon = false;
00273         FD_ZERO(&set);
00274         FD_SET(joy_fd, &set);
00275         
00276         //ROS_INFO("Select...");
00277         int select_out = select(joy_fd+1, &set, NULL, NULL, &tv);
00278         //ROS_INFO("Tick...");
00279         if (select_out == -1)
00280         {
00281           tv.tv_sec = 0;
00282           tv.tv_usec = 0;
00283           //ROS_INFO("Select returned negative. %i", ros::isShuttingDown());
00284           continue;
00285           //break; // Joystick is probably closed. Not sure if this case is useful.
00286         }
00287         
00288         if (FD_ISSET(joy_fd, &set))
00289         {
00290           if (read(joy_fd, &event, sizeof(js_event)) == -1 && errno != EAGAIN)
00291             break; // Joystick is probably closed. Definitely occurs.
00292           
00293           //ROS_INFO("Read data...");
00294           joy_msg.header.stamp = ros::Time().now();
00295           event_count_++;
00296           switch(event.type)
00297           {
00298           case JS_EVENT_BUTTON:
00299           case JS_EVENT_BUTTON | JS_EVENT_INIT:
00300             if(event.number >= joy_msg.buttons.size())
00301             {
00302               int old_size = joy_msg.buttons.size();
00303               joy_msg.buttons.resize(event.number+1);
00304               last_published_joy_msg.buttons.resize(event.number+1);
00305               sticky_buttons_joy_msg.buttons.resize(event.number+1);
00306               for(unsigned int i=old_size;i<joy_msg.buttons.size();i++){
00307                 joy_msg.buttons[i] = 0.0;
00308                 last_published_joy_msg.buttons[i] = 0.0;
00309                 sticky_buttons_joy_msg.buttons[i] = 0.0;
00310               }
00311             }
00312             joy_msg.buttons[event.number] = (event.value ? 1 : 0);
00313             // For initial events, wait a bit before sending to try to catch
00314             // all the initial events.
00315             if (!(event.type & JS_EVENT_INIT))
00316               publish_now = true;
00317             else
00318               publish_soon = true;
00319             break;
00320           case JS_EVENT_AXIS:
00321           case JS_EVENT_AXIS | JS_EVENT_INIT:
00322             val = event.value;
00323             if(event.number >= joy_msg.axes.size())
00324             {
00325               int old_size = joy_msg.axes.size();
00326               joy_msg.axes.resize(event.number+1);
00327               last_published_joy_msg.axes.resize(event.number+1);
00328               sticky_buttons_joy_msg.axes.resize(event.number+1);
00329               for(unsigned int i=old_size;i<joy_msg.axes.size();i++){
00330                 joy_msg.axes[i] = 0.0;
00331                 last_published_joy_msg.axes[i] = 0.0;
00332                 sticky_buttons_joy_msg.axes[i] = 0.0;
00333               }
00334             }
00335             if(default_trig_val_){ 
00336               // Allows deadzone to be "smooth"
00337               if (val > unscaled_deadzone)
00338                 val -= unscaled_deadzone;
00339               else if (val < -unscaled_deadzone)
00340                 val += unscaled_deadzone;
00341               else
00342                 val = 0;
00343               joy_msg.axes[event.number] = val * scale;
00344               // Will wait a bit before sending to try to combine events.                               
00345               publish_soon = true;
00346               break;
00347             } 
00348             else 
00349             {
00350               if (!(event.type & JS_EVENT_INIT))
00351               {
00352                 val = event.value;
00353                 if(val > unscaled_deadzone)
00354                   val -= unscaled_deadzone;
00355                 else if(val < -unscaled_deadzone)
00356                   val += unscaled_deadzone;
00357                 else
00358                   val=0;
00359                 joy_msg.axes[event.number]= val * scale;
00360               }
00361 
00362               publish_soon = true;
00363               break;
00364               default:
00365               ROS_WARN("joy_node: Unknown event type. Please file a ticket. time=%u, value=%d, type=%Xh, number=%d", event.time, event.value, event.type, event.number);
00366               break;
00367             }
00368           }
00369         }
00370         else if (tv_set) // Assume that the timer has expired.
00371           publish_now = true;
00372 
00373         if (publish_now) {
00374           // Assume that all the JS_EVENT_INIT messages have arrived already.
00375           // This should be the case as the kernel sends them along as soon as
00376           // the device opens.
00377           //ROS_INFO("Publish...");
00378           if (sticky_buttons_ == true) {
00379             // cycle through buttons
00380             for (size_t i = 0; i < joy_msg.buttons.size(); i++) {
00381               // change button state only on transition from 0 to 1
00382               if (joy_msg.buttons[i] == 1 && last_published_joy_msg.buttons[i] == 0) {
00383                 sticky_buttons_joy_msg.buttons[i] = sticky_buttons_joy_msg.buttons[i] ? 0 : 1;
00384               } else {
00385                 // do not change the message sate
00386                 //sticky_buttons_joy_msg.buttons[i] = sticky_buttons_joy_msg.buttons[i] ? 0 : 1;
00387               }
00388             }
00389             // update last published message
00390             last_published_joy_msg = joy_msg;
00391             // fill rest of sticky_buttons_joy_msg (time stamps, axes, etc)
00392             sticky_buttons_joy_msg.header.stamp.nsec = joy_msg.header.stamp.nsec;
00393             sticky_buttons_joy_msg.header.stamp.sec  = joy_msg.header.stamp.sec;
00394             sticky_buttons_joy_msg.header.frame_id   = joy_msg.header.frame_id;
00395             for(size_t i=0; i < joy_msg.axes.size(); i++){
00396               sticky_buttons_joy_msg.axes[i] = joy_msg.axes[i];
00397             }
00398             pub_.publish(sticky_buttons_joy_msg);
00399           } else {
00400             pub_.publish(joy_msg);
00401           }
00402 
00403           publish_now = false;
00404           tv_set = false;
00405           publication_pending = false;
00406           publish_soon = false;
00407           pub_count_++;
00408         }
00409 
00410         // If an axis event occurred, start a timer to combine with other
00411         // events.
00412         if (!publication_pending && publish_soon)
00413         {
00414           tv.tv_sec = trunc(coalesce_interval_);
00415           tv.tv_usec = (coalesce_interval_ - tv.tv_sec) * 1e6;
00416           publication_pending = true;
00417           tv_set = true;
00418           //ROS_INFO("Pub pending...");
00419         }
00420         
00421         // If nothing is going on, start a timer to do autorepeat.
00422         if (!tv_set && autorepeat_rate_ > 0)
00423         {
00424           tv.tv_sec = trunc(autorepeat_interval);
00425           tv.tv_usec = (autorepeat_interval - tv.tv_sec) * 1e6; 
00426           tv_set = true;
00427           //ROS_INFO("Autorepeat pending... %li %li", tv.tv_sec, tv.tv_usec);
00428         }
00429         
00430         if (!tv_set)
00431         {
00432           tv.tv_sec = 1;
00433           tv.tv_usec = 0;
00434         }
00435         
00436         diagnostic_.update();
00437       } // End of joystick open loop.
00438       
00439       close(joy_fd);
00440       ros::spinOnce();
00441       if (nh_.ok())
00442         ROS_ERROR("Connection to joystick device lost unexpectedly. Will reopen.");
00443     }
00444     
00445   cleanup:
00446     ROS_INFO("joy_node shut down.");
00447     
00448     return 0;
00449   }
00450 };
00451 
00452 int main(int argc, char **argv)
00453 {
00454   ros::init(argc, argv, "joy_node");
00455   Joystick j;
00456   return j.main(argc, argv);
00457 }


joy
Author(s): Morgan Quigley, Brian Gerkey, Kevin Watts, Blaise Gassend
autogenerated on Sun Jul 9 2017 02:34:54