creating joystick_drivers stack and populating

git-svn-id: https://code.ros.org/svn/ros-pkg/pkg/trunk/stacks/joystick_drivers@17568 eb33c2ac-9c88-4c90-87e0-44a10359b0c3

joy/CMakeLists.txt

@@ -0,0 +1,15 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+rospack(joy)
+genmsg()
+
+# Look for <linux/joystick.h>
+include(CheckIncludeFiles)
+check_include_files(linux/joystick.h HAVE_LINUX_JOYSTICK_H)
+if(HAVE_LINUX_JOYSTICK_H)
+  include_directories(msg/cpp)
+  rospack_add_executable(joy joy.cpp)
+  rospack_add_executable(txjoy txjoy.cpp)
+else(HAVE_LINUX_JOYSTICK_H)
+  message("Warning: no <linux/joystick.h>; won't build joy node")
+endif(HAVE_LINUX_JOYSTICK_H)

joy/Makefile

@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk

joy/joy.cpp

@@ -0,0 +1,102 @@
+#include <unistd.h>
+#include <math.h>
+#include <linux/joystick.h>
+#include <fcntl.h>
+#include "ros/node.h"
+#include "joy/Joy.h"
+
+using namespace std;
+
+void *s_joy_func(void *);
+using namespace ros;
+
+class Joy : public Node
+{
+public:
+  joy::Joy joy_msg;
+  int joy_fd;
+  string joy_dev;
+  int deadzone;
+  pthread_t joy_thread;
+
+  Joy() : Node("joy")
+  {
+    param<string>("~dev", joy_dev, "/dev/input/js0");
+    param<int>("~deadzone", deadzone, 2000);
+    printf("dev:%s\n", joy_dev.c_str());
+    printf("deadzone:%d\n", deadzone);
+    advertise<joy::Joy>("joy",10);
+  }
+  void start()
+  {
+    joy_fd = open(joy_dev.c_str(), O_RDONLY);
+    if (joy_fd <= 0)
+    {
+      ROS_FATAL("couldn't open joystick %s.", joy_dev.c_str());
+      ROS_BREAK();
+    }
+    pthread_create(&joy_thread, NULL, s_joy_func, this);
+  }
+  void stop()
+  {
+    pthread_cancel(joy_thread);
+    pthread_join(joy_thread,NULL);
+    close(joy_fd);
+  }
+  void joy_func()
+  {
+    js_event event;
+    while (ok())
+    {
+      pthread_testcancel();
+      read(joy_fd, &event, sizeof(js_event));
+      if (event.type & JS_EVENT_INIT)
+        continue;
+      switch(event.type)
+      {
+        case JS_EVENT_BUTTON:
+          if(event.number >= joy_msg.get_buttons_size())
+          {
+            joy_msg.set_buttons_size(event.number+1);
+            for(unsigned int i=0;i<joy_msg.get_buttons_size();i++)
+              joy_msg.buttons[i] = 0;
+          }
+          if(event.value)
+            joy_msg.buttons[event.number] = 1;
+          else
+            joy_msg.buttons[event.number] = 0;
+          publish("joy", joy_msg);
+          break;
+        case JS_EVENT_AXIS:
+          if(event.number >= joy_msg.get_axes_size())
+          {
+            joy_msg.set_axes_size(event.number+1);
+            for(unsigned int i=0;i<joy_msg.get_axes_size();i++)
+              joy_msg.axes[i] = 0.0;
+          }
+          joy_msg.axes[event.number] = (fabs(event.value) < deadzone) ? 0.0 :
+                  (-event.value / 32767.0);
+          cout << "axis: " << (int) event.number << ", value: " << joy_msg.axes[event.number] << endl;
+          publish("joy", joy_msg);
+          break;
+      }
+    }
+  }
+};
+
+void *s_joy_func(void *parent)
+{
+  ((Joy *)parent)->joy_func();
+  return NULL;
+}
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv);
+  Joy joy;
+  joy.start();
+  joy.spin();
+  joy.stop();
+  return 0;
+}
+

joy/manifest.xml

@@ -0,0 +1,9 @@
+<package>
+  <author>Morgan Quigley and Brian Gerkey</author>
+  <license>BSD</license>
+  <review status="unreviewed" notes=""/>
+  <depend package="roscpp"/>
+  <export>
+    <cpp cflags="-I${prefix}/msg/cpp" />
+  </export>
+</package>

joy/msg/Joy.msg

@@ -0,0 +1,2 @@
+float32[] axes
+int32[] buttons

joy/txjoy.cpp

@@ -0,0 +1,67 @@
+#include <unistd.h>
+#include <math.h>
+#include <linux/joystick.h>
+#include <fcntl.h>
+#include "ros/ros.h"
+#include "joy/Joy.h"
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "txjoy");
+  ros::NodeHandle n;
+  ros::Publisher pub = n.advertise<joy::Joy>("joy", 1);
+  std::string joy_dev;
+  int deadzone = 0;
+  n.param<std::string>("~dev", joy_dev, "/dev/input/js0");
+  n.param<int>("~deadzone", deadzone, 2000);
+  int joy_fd = open(joy_dev.c_str(), O_RDONLY);
+  if (joy_fd <= 0)
+  {
+    ROS_FATAL("couldn't open joystick %s.", joy_dev.c_str());
+    ROS_BREAK();
+  }
+  joy::Joy joy_msg;
+  js_event event;
+  struct timeval tv;
+  fd_set set;
+  while (n.ok())
+  {
+    tv.tv_sec = 0;
+    tv.tv_usec = 100000; // wait 100ms for something to happen
+    FD_ZERO(&set);
+    FD_SET(joy_fd, &set);
+    if (select(joy_fd+1, &set, NULL, NULL, &tv) == 1 && FD_ISSET(joy_fd, &set))
+    {
+      read(joy_fd, &event, sizeof(js_event));
+      if (event.type & JS_EVENT_INIT)
+        continue;
+      switch(event.type)
+      {
+        case JS_EVENT_BUTTON:
+          if(event.number >= joy_msg.get_buttons_size())
+          {
+            joy_msg.set_buttons_size(event.number+1);
+            for(unsigned int i=0;i<joy_msg.get_buttons_size();i++)
+              joy_msg.buttons[i] = 0;
+          }
+          joy_msg.buttons[event.number] = (event.value ? 1 : 0);
+          pub.publish(joy_msg);
+          break;
+        case JS_EVENT_AXIS:
+          if(event.number >= joy_msg.get_axes_size())
+          {
+            joy_msg.set_axes_size(event.number+1);
+            for(unsigned int i=0;i<joy_msg.get_axes_size();i++)
+              joy_msg.axes[i] = 0.0;
+          }
+          joy_msg.axes[event.number] = (fabs(event.value) < deadzone) ? 0.0 :
+            (-event.value / 32767.0);
+          pub.publish(joy_msg);
+          break;
+      }
+    }
+  }
+  close(joy_fd);
+  return 0;
+}
+