evdev_listener.cpp
Go to the documentation of this file.
00001 
00002 // Using the Input Subsystem, Part II
00003 
00004 // http://www.linuxquestions.org/questions/linux-desktop-74/hal-vs-xserver-dev-input-mice-convincing-x-to-*not*-use-a-given-input-device-697802/
00005 
00006 // Wouse : Wince mouse.  Input device that detects a user wincing to produce some type of event.
00007 //  In this case the wouse is used as to produce a runstop event for a PR2.
00008 
00009 // This node uses events from Swiftpoint mouse to provide runstop signal as message on ROS topic.
00010 // Node is meant to be used as safety mechanism, extra considuration is required:
00011 //  1. What happeds if node crashes or wouse runs out of batteries or gets unplugged?
00012 //  2. If networking buffers/delays messages.  IE, target system ROS messages but they are delayed by 1 or more seconds.
00013     
00014 // Currently it is possible, to make a ROS service call that will halt motors on a PR2.  
00015 // This is not a reliable runstop method b 
00016 
00017 // Pulish data every <pub_timeout> seconds, even if button state has not changed
00018 // this should allow rostopic message to work as a runstop keep-alive
00019 // if signal does
00020 
00021 #include <linux/input.h>
00022 #include <sys/types.h>
00023 #include <sys/stat.h>
00024 #include <fcntl.h>
00025 #include <stdio.h>
00026 #include <stdlib.h>
00027 #include <errno.h>
00028 #include <stdint.h>
00029 #include <string.h>
00030 #include <unistd.h>
00031 #include <sys/select.h>
00032 
00033 #include <ros/ros.h>
00034 #include <std_msgs/Int32.h>
00035 #include <geometry_msgs/Vector3Stamped.h>
00036 #include <wouse/evdev_listener.h>
00037 
00038 #include <sstream>
00039 
00040 namespace evdev_listener
00041 {
00042 
00043 EventInput::EventInput() 
00044   : fd_(-1)
00045   , initialized_(false)
00046 {
00047 };
00048 
00049 bool EventInput::init(const char *input_device, ros::NodeHandle nh)
00050 {
00051   ROS_INFO("Opening device '%s'", input_device);
00052   fd_ = open(input_device, O_RDONLY);
00053   if (fd_ == -1)
00054   {
00055     int error = errno;
00056     ROS_ERROR("Opening device %s : %s", input_device, strerror(error));
00057     return false;
00058   }
00059 
00060   /* ioctl() accesses the underlying driver */
00061   uint32_t version;
00062   if (ioctl(fd_, EVIOCGVERSION, &version))
00063   {    
00064     int error = errno;
00065     if (error == ENOTTY)
00066     {
00067       ROS_ERROR("'%s' does not seem to be an input event device file. "
00068                 "Input event devices are often named '/dev/input/eventX' where X is an integer.", 
00069                 input_device);
00070     }
00071     else
00072     {
00073       ROS_ERROR("EVIOCGVERSION ioctl failed : %s", strerror(error));
00074     }
00075     return false;
00076   }
00077 
00078   /* the EVIOCGVERSION ioctl() returns an int */
00079   /* so we unpack it and display it */
00080   ROS_INFO("Event driver version is %d.%d.%d",
00081            version >> 16, (version >> 8) & 0xff, version & 0xff);
00082   
00083   if (version != EV_VERSION)
00084   {
00085     version = EV_VERSION;
00086     ROS_WARN("This code was compiled deferent input device header version : %d.%d.%d",
00087              version >> 16, (version >> 8) & 0xff, version & 0xff);
00088   }
00089 
00090 
00091   // Get id : Id should help to determine is device actually a the Swiftpoint Mouse
00092   struct input_id id;
00093   if (ioctl(fd_, EVIOCGID, &id)) 
00094   {
00095     int error = errno;
00096     ROS_ERROR("EVIOCGID ioctl : %s", strerror(error));
00097     return false;
00098   }
00099 
00100   if (id.bustype != BUS_USB)
00101   {
00102     ROS_WARN("Input device does not seem to be a USB device. "
00103              "Expected bustype of %d, instead of %d.", 
00104              BUS_USB, id.bustype);
00105   }
00106   else if ((id.vendor != SWIFTPOINT_USB_VENDOR_ID) || (id.product != SWIFTPOINT_USB_PRODUCT_ID))
00107   {
00108     ROS_WARN("Input device vendor and product ID of %04X:%04X does not match expected value of %04X:%04X",
00109              int(id.vendor), int(id.product), SWIFTPOINT_USB_VENDOR_ID, SWIFTPOINT_USB_PRODUCT_ID);
00110   }           
00111            
00112   ROS_DEBUG("Input device id bustype:%u, vendor:%04X, product:%04X, version:%04X\n",
00113             (unsigned)id.bustype,
00114             (unsigned)id.vendor,
00115             (unsigned)id.product,
00116             (unsigned)id.version);
00117 
00118 
00119   // Get string description of device
00120   char buf[256];
00121   int buf_len;
00122 
00123 
00124   buf_len = ioctl(fd_, EVIOCGNAME((sizeof(buf)-1)), buf);
00125   if (buf_len < 0) 
00126   {
00127     int error = errno;
00128     ROS_WARN("EVIOCGNAME ioctl : %s", strerror(error));
00129   }
00130   else 
00131   {
00132     buf[buf_len] = '\0';  
00133     if (buf_len > 0)
00134     {
00135       ROS_INFO("Device name is '%s'", buf);
00136     }
00137     else
00138     {
00139       ROS_INFO("Device has no name (string length = 0)");
00140     }
00141   }
00142 
00143 
00144   buf_len = ioctl(fd_, EVIOCGPHYS(sizeof(buf)-1), buf);
00145   if (buf_len < 0) 
00146   {
00147     int error = errno;
00148     ROS_WARN("EVIOCGPHYS ioctl : %s", strerror(error));
00149     return false;
00150   }
00151   else 
00152   {
00153     buf[buf_len] = '\0';
00154     if (buf_len>0)
00155     {
00156       ROS_INFO("Device physical location is '%s'", buf);
00157     }
00158   }
00159 
00160 
00161   buf_len = ioctl(fd_, EVIOCGUNIQ(sizeof(buf)-1), buf);
00162   if (buf_len < 0) 
00163   {
00164     int error = errno;
00165     ROS_WARN("EVIOCGUNIQ ioctl : %s", strerror(error));
00166   }
00167   else 
00168   {
00169     buf[buf_len] = '\0';
00170     if (buf_len>0)
00171     {
00172       ROS_INFO("Unique identifier for device is '%s'", buf);
00173     }
00174     else 
00175     {
00176       ROS_INFO("No unique identifier for device");
00177     }
00178   }
00179 
00180 
00181   uint32_t event_bitflags = 0;
00182   if (ioctl(fd_, EVIOCGBIT(0, sizeof(event_bitflags)), &event_bitflags) < 0) 
00183   {
00184     int error = errno;
00185     ROS_ERROR("EVIOCGBIT ioctl : %s", strerror(error));    
00186     return false;
00187   }
00188   
00189   unsigned ev_max = EV_MAX;
00190   if (ev_max > (8*sizeof(event_bitflags)) )
00191   {
00192     ROS_WARN("ev_max > %u", unsigned(8*sizeof(event_bitflags)) );
00193     ev_max = 8*sizeof(event_bitflags);
00194   }
00195   
00196   std::ostringstream os;
00197   bool first = true;
00198   os << "Event types provided by device : ";
00199   for (unsigned ev_type_index = 0; ev_type_index < ev_max; ++ev_type_index) 
00200   {
00201     if ( (event_bitflags>>ev_type_index)&1 ) 
00202     {
00203       if (!first)
00204       {
00205         os << ", ";
00206       }
00207       first = false;
00208       /* the bit is set in the event types list */
00209       switch (ev_type_index)
00210       {
00211       case EV_SYN :
00212         os << "Synchronization";
00213         break;
00214       case EV_KEY :
00215         os << "Keys/Buttons";
00216         break;
00217       case EV_REL :
00218         os << "Relative Axes";
00219         break;
00220       case EV_ABS :
00221         os << "Absolute Axes";
00222         break;
00223       case EV_MSC :
00224         os << "Miscellaneous";
00225         break;
00226       case EV_LED :
00227         os << "LEDs";
00228         break;
00229       case EV_SND :
00230         os << "Sounds";
00231         break;
00232       case EV_REP :
00233         os << "Repeat";
00234         break;
00235       case EV_FF :
00236       case EV_FF_STATUS:
00237         os << "Force Feedback";
00238         break;
00239       case EV_PWR:
00240         os << "Power Management";
00241         break;
00242       default:
00243         os << "Unknown(" << ev_type_index << ")";
00244       }
00245     }
00246   }
00247   
00248   if ( (event_bitflags & (1<<EV_SYN))  &&  (event_bitflags & (1<<EV_REL)) )
00249   {
00250     ROS_INFO_STREAM(os.str());
00251   }
00252   else
00253   {
00254     ROS_ERROR("Device must provide 'Synchronization' and 'Relative Axes' events for driver to work. %s", 
00255               os.str().c_str());
00256     return false;
00257   }
00258 
00259 
00260   // make fd non-blocking
00261   int flags = fcntl(fd_, F_GETFL);
00262   if (flags == -1)
00263   {
00264     int error = errno;
00265     ROS_ERROR("Getting file descriptor flags : %s", strerror(error));
00266     return false;
00267   }  
00268   if (fcntl(fd_, F_SETFL, flags|O_NONBLOCK) == -1)
00269   {
00270     int error = errno;
00271     ROS_ERROR("Setting file descriptor flags : %s", strerror(error));
00272     return false;
00273   }
00274 
00275   // Steal mouse away from operating system so it does cause mouse movements
00276   int grab = 1;
00277   if (ioctl(fd_, EVIOCGRAB, &grab) < 0)
00278   {
00279     int error = errno;
00280     ROS_ERROR("EVIOCGRAB ioctl : %s", strerror(error));
00281     return false;
00282   } 
00283 
00284   movement_pub_ = nh.advertise<geometry_msgs::Vector3Stamped>("wouse_movement", 100);
00285 
00286   initialized_ = true;
00287   return true;
00288 }
00289 
00290 
00291 
00292 bool EventInput::run()
00293 {
00294   if (!initialized_)
00295   {
00296     return false;
00297   }
00298   ROS_INFO("[evdev_listener]: Ready");
00299   int num_event_slots = 64;
00300   struct input_event *event_buf = new input_event[num_event_slots];
00301   fd_set read_set, except_set;
00302   struct timeval timeout;
00303   double delta_x=0, delta_y=0;  // keep track of delta positions between event.
00304   bool first = true;
00305 
00306   ros::Duration wince_publish_duration(0.1);
00307  
00308   bool is_error = false;
00309 
00310   while (!is_error && ros::ok())
00311   {
00312     FD_ZERO(&read_set);    
00313     FD_ZERO(&except_set);    
00314     FD_SET(fd_, &read_set);
00315     FD_SET(fd_, &except_set);
00316     timeout.tv_sec = 0; 
00317     timeout.tv_usec = 10 * 1000; // 100ms = 1/10th second
00318     int result = select(fd_+1, &read_set, NULL, &except_set, &timeout);
00319     if (result < 0)
00320     {
00321       int error = errno;
00322       ROS_ERROR("Select : %s", strerror(error)); 
00323       is_error = true;
00324     }
00325     else if (result == 0)
00326     {
00327       //timeout -- no new data      
00328       // ROS_WARN("Timeout");
00329     }
00330     else 
00331     {
00332       // either read set of 
00333       if (FD_ISSET(fd_, &except_set))
00334       {
00335         ROS_WARN("Exception set");
00336       }
00337       if (!FD_ISSET(fd_, &read_set))
00338       {
00339         ROS_WARN("Read bit not set");
00340       }
00341      
00342       size_t read_len = read(fd_,event_buf,sizeof(struct input_event)*num_event_slots);
00343       if (int(read_len) == -1)
00344       {
00345         int error = errno;
00346         ROS_ERROR("Read : %s", strerror(error));
00347         is_error = true;
00348       }
00349       else if ( (read_len%sizeof(struct input_event) ) != 0)
00350       {
00351         ROS_ERROR("%d leftover bytes after read",
00352                   int(read_len%sizeof(struct input_event)));
00353         is_error = true;
00354       }
00355       else 
00356       {
00357         int num_events = read_len / sizeof(struct input_event);
00358         for (int ii = 0; ii<num_events; ++ii)
00359         {
00360           const struct input_event &event(event_buf[ii]);          
00361           // swiftpoint mouse seems to support 4 event types:
00362           // EV_SYN, EV_KEY, EV_REL, and EV_MSC
00363           // For tracking mouse movements, we track REL movements for both X, and Y
00364           // And publish the data once we get a SYN (sync).
00365 
00366           // Use  event time since is will probabbly produce a more acurrate
00367           // duration and movement velocities than ros::Time::now()
00368           ros::Time current_event_time;
00369           current_event_time.sec = event.time.tv_sec;
00370           current_event_time.nsec = event.time.tv_usec * 1000;
00371           
00372           if (event.type == EV_SYN)
00373           {// Event Sync. Publish the previous delta_x, delta_y values
00374 
00375             publishMovement(delta_x, delta_y, current_event_time);
00376 
00377             first = false;
00378             delta_x = 0; delta_y=0;
00379           }
00380           else if ((event.type == EV_REL) && (event.code == REL_X))
00381           {
00382             delta_x = event.value;
00383           }
00384           else if ((event.type == EV_REL) && (event.code == REL_Y))
00385           {
00386             delta_y = event.value;
00387           }
00388           else
00389           { //debugging
00390              printf("%ld.%06ld ",
00391                     event.time.tv_sec,
00392                     event.time.tv_usec);
00393              printf(" type %d, code %d value %x\n",
00394                     event.type,event.code, event.value);   
00395           }
00396         } // end for
00397       } // end if(read_result)
00398     } //end select
00399   } //end while (ros::ok())
00400 
00401   // as node is exiting send a runstop message
00402   return is_error;
00403 }
00404 
00405 void EventInput::publishMovement(double delta_x, double delta_y, ros::Time time)
00406 {
00407   geometry_msgs::Vector3Stamped movement_msg;
00408   movement_msg.header.stamp = time;
00409   movement_msg.vector.x = delta_x;
00410   movement_msg.vector.y = delta_y;
00411   movement_msg.vector.z = 0;
00412   movement_pub_.publish(movement_msg);
00413 }
00414 
00415 }; //end namespace event_listener
00416 
00417 int main(int argc, char* argv[])
00418 {
00419   ros::init(argc, argv, "wouse_evdev_listener");
00420   ros::NodeHandle nh;
00421 
00422   if (argc < 2)
00423   {
00424     fprintf(stderr, "Please provide device path : ie /dev/wouse \n");
00425     exit(EXIT_FAILURE);
00426   }
00427 
00428   evdev_listener::EventInput evdev_listener;
00429   ROS_INFO("Preparing to init");
00430   
00431   if (!evdev_listener.init(argv[1], nh))
00432   {
00433     return 1;
00434   }
00435   ROS_INFO("Preparing to run");
00436   if (!evdev_listener.run())
00437   {
00438     return 1;
00439   }
00440 
00441   return 0;
00442 }


wouse
Author(s): Phillip M. Grice, Advisor: Prof. Charlie Kemp, Lab: The Healthcare Robotoics Lab at Georgia Tech.
autogenerated on Wed Nov 27 2013 11:57:42