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 <diagnostic_updater/diagnostic_updater.h>
00038 #include "ros/ros.h"
00039 #include <sensor_msgs/Joy.h>
00040 
00041 
00043 class Joystick
00044 {
00045 private:
00046   ros::NodeHandle nh_;
00047   bool open_;               
00048   std::string joy_dev_;
00049   double deadzone_;
00050   double autorepeat_rate_;  // in Hz.  0 for no repeat.
00051   double coalesce_interval_; // Defaults to 100 Hz rate limit.
00052   int event_count_;
00053   int pub_count_;
00054   ros::Publisher pub_;
00055   double lastDiagTime_;
00056   
00057   diagnostic_updater::Updater diagnostic_;
00058   
00060   void diagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat)
00061   {
00062     double now = ros::Time::now().toSec();
00063     double interval = now - lastDiagTime_;
00064     if (open_)
00065       stat.summary(0, "OK");
00066     else
00067       stat.summary(2, "Joystick not open.");
00068     
00069     stat.add("topic", pub_.getTopic());
00070     stat.add("device", joy_dev_);
00071     stat.add("dead zone", deadzone_);
00072     stat.add("autorepeat rate (Hz)", autorepeat_rate_);
00073     stat.add("coalesce interval (s)", coalesce_interval_);
00074     stat.add("recent joystick event rate (Hz)", event_count_ / interval);
00075     stat.add("recent publication rate (Hz)", pub_count_ / interval);
00076     stat.add("subscribers", pub_.getNumSubscribers());
00077     event_count_ = 0;
00078     pub_count_ = 0;
00079     lastDiagTime_ = now;
00080   }
00081   
00082 public:
00083   Joystick() : nh_(), diagnostic_()
00084   {}
00085   
00087   int main(int argc, char **argv)
00088   {
00089     diagnostic_.add("Joystick Driver Status", this, &Joystick::diagnostics);
00090     diagnostic_.setHardwareID("none");
00091 
00092     // Parameters
00093     ros::NodeHandle nh_param("~");
00094     pub_ = nh_.advertise<sensor_msgs::Joy>("joy", 1);
00095     nh_param.param<std::string>("dev", joy_dev_, "/dev/input/js0");
00096     nh_param.param<double>("deadzone", deadzone_, 0.05);
00097     nh_param.param<double>("autorepeat_rate", autorepeat_rate_, 0);
00098     nh_param.param<double>("coalesce_interval", coalesce_interval_, 0.001);
00099     
00100     // Checks on parameters
00101     if (autorepeat_rate_ > 1 / coalesce_interval_)
00102       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_);
00103     
00104     if (deadzone_ >= 1)
00105     {
00106       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.");
00107       deadzone_ /= 32767;
00108     }
00109     
00110     if (deadzone_ > 0.9)
00111     {
00112       ROS_WARN("joy_node: deadzone (%f) greater than 0.9, setting it to 0.9", deadzone_);
00113       deadzone_ = 0.9;
00114     }
00115     
00116     if (deadzone_ < 0)
00117     {
00118       ROS_WARN("joy_node: deadzone_ (%f) less than 0, setting to 0.", deadzone_);
00119       deadzone_ = 0;
00120     }
00121 
00122     if (autorepeat_rate_ < 0)
00123     {
00124       ROS_WARN("joy_node: autorepeat_rate (%f) less than 0, setting to 0.", autorepeat_rate_);
00125       autorepeat_rate_ = 0;
00126     }
00127     
00128     if (coalesce_interval_ < 0)
00129     {
00130       ROS_WARN("joy_node: coalesce_interval (%f) less than 0, setting to 0.", coalesce_interval_);
00131       coalesce_interval_ = 0;
00132     }
00133     
00134     // Parameter conversions
00135     double autorepeat_interval = 1 / autorepeat_rate_;
00136     double scale = -1. / (1. - deadzone_) / 32767.;
00137     double unscaled_deadzone = 32767. * deadzone_;
00138 
00139     js_event event;
00140     struct timeval tv;
00141     fd_set set;
00142     int joy_fd;
00143     event_count_ = 0;
00144     pub_count_ = 0;
00145     lastDiagTime_ = ros::Time::now().toSec();
00146     
00147     // Big while loop opens, publishes
00148     while (nh_.ok())
00149     {                                      
00150       open_ = false;
00151       diagnostic_.force_update();
00152       bool first_fault = true;
00153       while (true)
00154       {
00155         ros::spinOnce();
00156         if (!nh_.ok())
00157           goto cleanup;
00158         joy_fd = open(joy_dev_.c_str(), O_RDONLY);
00159         if (joy_fd != -1)
00160         {
00161           // There seems to be a bug in the driver or something where the
00162           // initial events that are to define the initial state of the
00163           // joystick are not the values of the joystick when it was opened
00164           // but rather the values of the joystick when it was last closed.
00165           // Opening then closing and opening again is a hack to get more
00166           // accurate initial state data.
00167           close(joy_fd);
00168           joy_fd = open(joy_dev_.c_str(), O_RDONLY);
00169         }
00170         if (joy_fd != -1)
00171           break;
00172         if (first_fault)
00173         {
00174           ROS_ERROR("Couldn't open joystick %s. Will retry every second.", joy_dev_.c_str());
00175           first_fault = false;
00176         }
00177         sleep(1.0);
00178         diagnostic_.update();
00179       }
00180       
00181       ROS_INFO("Opened joystick: %s. deadzone_: %f.", joy_dev_.c_str(), deadzone_);
00182       open_ = true;
00183       diagnostic_.force_update();
00184       
00185       bool tv_set = false;
00186       bool publication_pending = false;
00187       tv.tv_sec = 1;
00188       tv.tv_usec = 0;
00189       sensor_msgs::Joy joy_msg; // Here because we want to reset it on device close.
00190       while (nh_.ok()) 
00191       {
00192         ros::spinOnce();
00193         
00194         bool publish_now = false;
00195         bool publish_soon = false;
00196         FD_ZERO(&set);
00197         FD_SET(joy_fd, &set);
00198         
00199         //ROS_INFO("Select...");
00200         int select_out = select(joy_fd+1, &set, NULL, NULL, &tv);
00201         //ROS_INFO("Tick...");
00202         if (select_out == -1)
00203         {
00204           tv.tv_sec = 0;
00205           tv.tv_usec = 0;
00206           //ROS_INFO("Select returned negative. %i", ros::isShuttingDown());
00207           continue;
00208           //                            break; // Joystick is probably closed. Not sure if this case is useful.
00209         }
00210         
00211         if (FD_ISSET(joy_fd, &set))
00212         {
00213           if (read(joy_fd, &event, sizeof(js_event)) == -1 && errno != EAGAIN)
00214             break; // Joystick is probably closed. Definitely occurs.
00215           
00216           //ROS_INFO("Read data...");
00217           joy_msg.header.stamp = ros::Time().now();
00218           event_count_++;
00219           switch(event.type)
00220           {
00221           case JS_EVENT_BUTTON:
00222           case JS_EVENT_BUTTON | JS_EVENT_INIT:
00223             if(event.number >= joy_msg.buttons.size())
00224             {
00225               int old_size = joy_msg.buttons.size();
00226               joy_msg.buttons.resize(event.number+1);
00227               for(unsigned int i=old_size;i<joy_msg.buttons.size();i++)
00228                 joy_msg.buttons[i] = 0.0;
00229             }
00230             joy_msg.buttons[event.number] = (event.value ? 1 : 0);
00231             // For initial events, wait a bit before sending to try to catch
00232             // all the initial events.
00233             if (!(event.type & JS_EVENT_INIT))
00234               publish_now = true;
00235             else
00236               publish_soon = true;
00237             break;
00238           case JS_EVENT_AXIS:
00239           case JS_EVENT_AXIS | JS_EVENT_INIT:
00240             if(event.number >= joy_msg.axes.size())
00241             {
00242               int old_size = joy_msg.axes.size();
00243               joy_msg.axes.resize(event.number+1);
00244               for(unsigned int i=old_size;i<joy_msg.axes.size();i++)
00245                 joy_msg.axes[i] = 0.0;
00246             }
00247             if (!(event.type & JS_EVENT_INIT)) // Init event.value is wrong.
00248             {
00249               double val = event.value;
00250               // Allows deadzone to be "smooth"
00251               if (val > unscaled_deadzone)
00252                 val -= unscaled_deadzone;
00253               else if (val < -unscaled_deadzone)
00254                 val += unscaled_deadzone;
00255               else
00256                 val = 0;
00257               joy_msg.axes[event.number] = val * scale;
00258             }
00259             // Will wait a bit before sending to try to combine events.                                 
00260             publish_soon = true;
00261             break;
00262           default:
00263             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);
00264             break;
00265           }
00266         }
00267         else if (tv_set) // Assume that the timer has expired.
00268           publish_now = true;
00269         
00270         if (publish_now)
00271         {
00272           // Assume that all the JS_EVENT_INIT messages have arrived already.
00273           // This should be the case as the kernel sends them along as soon as
00274           // the device opens.
00275           //ROS_INFO("Publish...");
00276           pub_.publish(joy_msg);
00277           publish_now = false;
00278           tv_set = false;
00279           publication_pending = false;
00280           publish_soon = false;
00281           pub_count_++;
00282         }
00283         
00284         // If an axis event occurred, start a timer to combine with other
00285         // events.
00286         if (!publication_pending && publish_soon)
00287         {
00288           tv.tv_sec = trunc(coalesce_interval_);
00289           tv.tv_usec = (coalesce_interval_ - tv.tv_sec) * 1e6;
00290           publication_pending = true;
00291           tv_set = true;
00292           //ROS_INFO("Pub pending...");
00293         }
00294         
00295         // If nothing is going on, start a timer to do autorepeat.
00296         if (!tv_set && autorepeat_rate_ > 0)
00297         {
00298           tv.tv_sec = trunc(autorepeat_interval);
00299           tv.tv_usec = (autorepeat_interval - tv.tv_sec) * 1e6; 
00300           tv_set = true;
00301           //ROS_INFO("Autorepeat pending... %i %i", tv.tv_sec, tv.tv_usec);
00302         }
00303         
00304         if (!tv_set)
00305         {
00306           tv.tv_sec = 1;
00307           tv.tv_usec = 0;
00308         }
00309         
00310         diagnostic_.update();
00311       } // End of joystick open loop.
00312       
00313       close(joy_fd);
00314       ros::spinOnce();
00315       if (nh_.ok())
00316         ROS_ERROR("Connection to joystick device lost unexpectedly. Will reopen.");
00317     }
00318     
00319   cleanup:
00320     ROS_INFO("joy_node shut down.");
00321     
00322     return 0;
00323   }
00324 };
00325 
00326 int main(int argc, char **argv)
00327 {
00328   ros::init(argc, argv, "joy_node");
00329   Joystick j;
00330   return j.main(argc, argv);
00331 }


joy
Author(s): Morgan Quigley, Brian Gerkey, Kevin Watts, Blaise Gassend
autogenerated on Thu Aug 27 2015 13:35:56