hydra.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 *
00003 * This is free and unencumbered software released into the public domain.
00004 * 
00005 * Anyone is free to copy, modify, publish, use, compile, sell, or
00006 * distribute this software, either in source code form or as a compiled
00007 * binary, for any purpose, commercial or non-commercial, and by any
00008 * means.
00009 * 
00010 * In jurisdictions that recognize copyright laws, the author or authors
00011 * of this software dedicate any and all copyright interest in the
00012 * software to the public domain. We make this dedication for the benefit
00013 * of the public at large and to the detriment of our heirs and
00014 * successors. We intend this dedication to be an overt act of
00015 * relinquishment in perpetuity of all present and future rights to this
00016 * software under copyright law.
00017 * 
00018 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
00019 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
00020 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
00021 * IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR
00022 * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
00023 * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
00024 * OTHER DEALINGS IN THE SOFTWARE.
00025 * 
00026 * For more information, please refer to <http://unlicense.org/>
00027 * 
00028 **********************************************************************/
00029 
00030 #include "razer_hydra/hydra.h"
00031 #include <tf/tf.h>  // could remove TF dependency if we use geometry_msgs
00032 #include "ros/console.h"
00033 #include "ros/assert.h"
00034 
00035 #include <errno.h>
00036 #include <libusb-1.0/libusb.h>
00037 #include <linux/types.h>
00038 #include <linux/input.h>
00039 #include <linux/hidraw.h>
00040 #include <cstring>
00041 
00042 // loosely adapted from the following 
00043 // https://github.com/rpavlik/vrpn/blob/razer-hydra/vrpn_Tracker_RazerHydra.C
00044 
00045 // and with reference to
00046 // http://lxr.free-electrons.com/source/samples/hidraw/hid-example.c
00047 
00048 /*
00049  * Ugly hack to work around failing compilation on systems that don't
00050  * yet populate new version of hidraw.h to userspace.
00051  *
00052  * If you need this, please have your distro update the kernel headers.
00053  */
00054 #ifndef HIDIOCSFEATURE
00055 #define HIDIOCSFEATURE(len)    _IOC(_IOC_WRITE|_IOC_READ, 'H', 0x06, len)
00056 #define HIDIOCGFEATURE(len)    _IOC(_IOC_WRITE|_IOC_READ, 'H', 0x07, len)
00057 #endif
00058 
00059 // eventually crawl hidraw file system using this:
00060 // http://www.signal11.us/oss/udev/
00061 
00062 
00063 namespace razer_hydra {
00064 
00065 RazerHydra::RazerHydra()
00066   : hidraw_fd(0)
00067 {
00068   ros::Time::init();
00069   last_cycle_start = ros::WallTime::now();
00070   period_estimate.setFc(0.11, 1.0); // magic number for 50% mix at each step
00071   period_estimate.setValue(0.004);
00072 
00073   for (int i = 0; i < 2; i++)
00074   {
00075     pos[i] = tf::Vector3(0,0,0);
00076     quat[i] = tf::Quaternion(0,0,0,1);
00077   }
00078 }
00079 
00080 RazerHydra::~RazerHydra()
00081 {
00082   if (hidraw_fd >= 0)
00083   {
00084     ROS_DEBUG("releasing hydra");
00085     uint8_t buf[256];
00086     memset(buf, 0, sizeof(buf));
00087     buf[6] = 1;
00088     buf[8] = 4;
00089     buf[89] = 5;
00090     int res = ioctl(hidraw_fd, HIDIOCSFEATURE(91), buf);
00091     if (res < 0)
00092     {
00093       ROS_ERROR("unable to stop streaming");
00094       perror("HIDIOCSFEATURE");
00095     }
00096     else
00097       ROS_DEBUG("stopped streaming");
00098     close(hidraw_fd);
00099   }
00100 }
00101 
00102 bool RazerHydra::init(const char *device)
00103 {
00104     int res;
00105     uint8_t buf[256];
00106     struct hidraw_report_descriptor rpt_desc;
00107     struct hidraw_devinfo info;
00108 
00109     hidraw_fd = open(device, O_RDWR | O_NONBLOCK);
00110     if (hidraw_fd < 0)
00111     {
00112         ROS_ERROR("couldn't open hidraw device");
00113         return false;
00114     }
00115     ROS_DEBUG("%s", (std::string("opened device") + std::string(device)).c_str());
00116 
00117     memset(&rpt_desc, 0x0, sizeof(rpt_desc));
00118     memset(&info,     0x0, sizeof(info));
00119     memset(buf,       0x0, sizeof(buf));
00120 
00121     /*
00122     // Get Report Descriptor Size
00123     ROS_DEBUG("Getting Report Descriptor Size");
00124     res = ioctl(hidraw_fd, HIDIOCGRDESCSIZE, &desc_size);
00125     if (res < 0)
00126         perror("HIDIOCGRDESCSIZE");
00127     else
00128         printf("Report Descriptor Size: %d\n", desc_size);
00129 
00130     // Get Report Descriptor
00131     ROS_DEBUG("Getting Report Descriptor");
00132     rpt_desc.size = desc_size;
00133     res = ioctl(hidraw_fd, HIDIOCGRDESC, &rpt_desc);
00134     if (res < 0) {
00135         perror("HIDIOCGRDESC");
00136     } else {
00137         printf("Report Descriptor:\n");
00138         for (size_t i = 0; i < rpt_desc.size; i++)
00139         printf("%hhx ", rpt_desc.value[i]);
00140         puts("\n");
00141     }
00142     */
00143 
00144     /* Get Raw Name */
00145     ROS_DEBUG("Getting Raw Name");
00146     res = ioctl(hidraw_fd, HIDIOCGRAWNAME(256), buf);
00147     if (res < 0)
00148         perror("HIDIOCGRAWNAME");
00149     else
00150         ROS_DEBUG("Raw Name: %s\n", buf);
00151 
00152     // set feature to start it streaming
00153     memset(buf, 0x0, sizeof(buf));
00154     buf[6] = 1;
00155     buf[8] = 4;
00156     buf[9] = 3;
00157     buf[89] = 6;
00158     int attempt = 0;
00159     for (; attempt < 50; attempt++)
00160     {
00161         res = ioctl(hidraw_fd, HIDIOCSFEATURE(91), buf);
00162         if (res < 0)
00163         {
00164             ROS_ERROR("unable to start streaming");
00165             perror("HIDIOCSFEATURE");
00166             usleep(50000);
00167         }
00168         else
00169         {
00170             ROS_DEBUG("Device stream is working.");
00171             break;
00172         }
00173     }
00174     ROS_DEBUG("%d attempts", attempt);
00175     return attempt < 60;
00176 }
00177 
00178 bool RazerHydra::poll(uint32_t ms_to_wait, float low_pass_corner_hz)
00179 {
00180   if (hidraw_fd < 0)
00181   {
00182     ROS_ERROR("hidraw device is not open, couldn't poll.");
00183     return false;
00184   }
00185   if(ms_to_wait == 0)
00186   {
00187     ROS_ERROR("ms_to_wait must be at least 1.");
00188     return false;
00189   }
00190   if(low_pass_corner_hz <= std::numeric_limits<float>::epsilon())
00191   {
00192     ROS_ERROR("Corner frequency for low-pass filter must be greater than 0. Aborting.");
00193     return false;
00194   }
00195   ros::WallTime t_start(ros::WallTime::now());
00196   ros::WallTime t_deadline(t_start + ros::WallDuration(0.001 * ms_to_wait));
00197 
00198   uint8_t buf[64];
00199   while (ros::WallTime::now() < t_deadline)
00200   {
00201     ssize_t nread = read(hidraw_fd, buf, sizeof(buf));
00202     ROS_DEBUG("read %d bytes", (int)nread);
00203     if (nread > 0)
00204     {
00205       static bool first_time = true;
00206       // Update average read period
00207       if(!first_time) {
00208         float last_period = (ros::WallTime::now() - last_cycle_start).toSec();
00209         period_estimate.process(last_period);
00210         ROS_DEBUG("last_cycle: %.4f sec", last_period);
00211       }
00212       last_cycle_start = ros::WallTime::now();
00213       if(first_time)
00214       {
00215         first_time = false;
00216         ROS_DEBUG("Got first data, everything should be working now!");
00217       }
00218 
00219       // Update filter frequencies
00220       float Fs = 1/period_estimate.getValue();
00221       float Fc = low_pass_corner_hz;
00222       for (int i = 0; i < 2; i++)
00223       {
00224         filter_pos[i].setFc(Fc, Fs);
00225         filter_quat[i].setFc(Fc, Fs);
00226       }
00227 
00228       // Read data
00229       raw_pos[0] = *((int16_t *)(buf+8));
00230       raw_pos[1] = *((int16_t *)(buf+10));
00231       raw_pos[2] = *((int16_t *)(buf+12));
00232       raw_quat[0] = *((int16_t *)(buf+14));
00233       raw_quat[1] = *((int16_t *)(buf+16));
00234       raw_quat[2] = *((int16_t *)(buf+18));
00235       raw_quat[3] = *((int16_t *)(buf+20));
00236       raw_buttons[0] = buf[22] & 0x7f;
00237       raw_analog[0] = *((int16_t *)(buf+23));
00238       raw_analog[1] = *((int16_t *)(buf+25));
00239       raw_analog[2] = buf[27];
00240 
00241       raw_pos[3] = *((int16_t *)(buf+30));
00242       raw_pos[4] = *((int16_t *)(buf+32));
00243       raw_pos[5] = *((int16_t *)(buf+34));
00244       raw_quat[4] = *((int16_t *)(buf+36));
00245       raw_quat[5] = *((int16_t *)(buf+38));
00246       raw_quat[6] = *((int16_t *)(buf+40));
00247       raw_quat[7] = *((int16_t *)(buf+42));
00248       raw_buttons[1] = buf[44] & 0x7f;
00249       raw_analog[3] = *((int16_t *)(buf+45));
00250       raw_analog[4] = *((int16_t *)(buf+47));
00251       raw_analog[5] = buf[49];
00252 
00253       // mangle the reported pose into the ROS frame conventions
00254       tf::Matrix3x3 ros_to_razer(  0, -1,  0,
00255                                   -1,  0,  0,
00256                                    0,  0, -1 );
00257       for (int i = 0; i < 2; i++)
00258       {
00259         pos[i].setX(raw_pos[3*i+0] * 0.001);
00260         pos[i].setY(raw_pos[3*i+1] * 0.001);
00261         pos[i].setZ(raw_pos[3*i+2] * 0.001);
00262         pos[i] = ros_to_razer*pos[i];
00263         
00264         tf::Quaternion q(raw_quat[i*4+1] / 32768.0,
00265                          raw_quat[i*4+2] / 32768.0,
00266                          raw_quat[i*4+3] / 32768.0,
00267                          raw_quat[i*4+0] / 32768.0);
00268         
00269         tf::Matrix3x3 mat(q);
00270         mat = ros_to_razer*mat*tf::Matrix3x3(tf::createQuaternionFromRPY(0, 0, M_PI_2));
00271         mat.getRotation(quat[i]);
00272       }
00273 
00274       // Apply filters
00275       for (int i = 0; i < 2; i++)
00276       {
00277         quat[i] = filter_quat[i].process(quat[i]);
00278         pos[i] = filter_pos[i].process(pos[i]);
00279       }
00280 
00281       analog[0] = raw_analog[0] / 32768.0;
00282       analog[1] = raw_analog[1] / 32768.0;
00283       analog[2] = raw_analog[2] / 255.0;
00284       analog[3] = raw_analog[3] / 32768.0;
00285       analog[4] = raw_analog[4] / 32768.0;
00286       analog[5] = raw_analog[5] / 255.0;
00287 
00288       for (int i = 0; i < 2; i++)
00289       {
00290         buttons[i*7  ] = (raw_buttons[i] & 0x01) ? 1 : 0;
00291         buttons[i*7+1] = (raw_buttons[i] & 0x04) ? 1 : 0;
00292         buttons[i*7+2] = (raw_buttons[i] & 0x08) ? 1 : 0;
00293         buttons[i*7+3] = (raw_buttons[i] & 0x02) ? 1 : 0;
00294         buttons[i*7+4] = (raw_buttons[i] & 0x10) ? 1 : 0;
00295         buttons[i*7+5] = (raw_buttons[i] & 0x20) ? 1 : 0;
00296         buttons[i*7+6] = (raw_buttons[i] & 0x40) ? 1 : 0;
00297       }
00298 
00299       return true;
00300     }
00301     else
00302     {
00303       ros::WallTime to_sleep = last_cycle_start + ros::WallDuration(period_estimate.getValue()*0.95);
00304       float sleep_duration = (to_sleep - ros::WallTime::now()).toSec();
00305       if(sleep_duration > 0)
00306       {
00307         ROS_DEBUG("Data not ready, sleeping for %.6f sec.  to_sleep %f, last_cycle_start = %f ", sleep_duration, to_sleep.toSec(), last_cycle_start.toSec());
00308         ros::WallTime::sleepUntil(to_sleep);
00309       }
00310       else {
00311         ROS_DEBUG("Data not ready, doing default sleep of 500 us");
00312         usleep(250);
00313       }
00314 
00315     }
00316   }
00317   ROS_DEBUG("Ran out of time, returning!");
00318   return false;
00319 }
00320 
00321 } //namespace


razer_hydra
Author(s): Adam Leeper
autogenerated on Fri Aug 28 2015 12:21:51