00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
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_;
00056 double coalesce_interval_;
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";
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
00109 if (strncmp(entry->d_name, "js", 2) != 0)
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))
00115 continue;
00116
00117
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
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
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
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
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
00236
00237
00238
00239
00240
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;
00264 double val;
00265 sensor_msgs::Joy last_published_joy_msg;
00266 sensor_msgs::Joy sticky_buttons_joy_msg;
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
00277 int select_out = select(joy_fd+1, &set, NULL, NULL, &tv);
00278
00279 if (select_out == -1)
00280 {
00281 tv.tv_sec = 0;
00282 tv.tv_usec = 0;
00283
00284 continue;
00285
00286 }
00287
00288 if (FD_ISSET(joy_fd, &set))
00289 {
00290 if (read(joy_fd, &event, sizeof(js_event)) == -1 && errno != EAGAIN)
00291 break;
00292
00293
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
00314
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
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
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)
00371 publish_now = true;
00372
00373 if (publish_now) {
00374
00375
00376
00377
00378 if (sticky_buttons_ == true) {
00379
00380 for (size_t i = 0; i < joy_msg.buttons.size(); i++) {
00381
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
00386
00387 }
00388 }
00389
00390 last_published_joy_msg = joy_msg;
00391
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
00411
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
00419 }
00420
00421
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
00428 }
00429
00430 if (!tv_set)
00431 {
00432 tv.tv_sec = 1;
00433 tv.tv_usec = 0;
00434 }
00435
00436 diagnostic_.update();
00437 }
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 }