From 65b4139aad9e7b17a0bde0ad5755a935c6d3027b Mon Sep 17 00:00:00 2001
From: Matthew Bries <matthew.bries@swri.org>
Date: Tue, 1 Aug 2017 17:15:40 -0500
Subject: [PATCH] Add basic force feedback over usb

Addresses #89
---
 joy/src/joy_node.cpp | 70 ++++++++++++++++++++++++++++++++++++++++++--
 1 file changed, 68 insertions(+), 2 deletions(-)

diff --git a/joy/src/joy_node.cpp b/joy/src/joy_node.cpp
index a64dde6d..eb32f17c 100644
--- a/joy/src/joy_node.cpp
+++ b/joy/src/joy_node.cpp
@@ -39,6 +39,9 @@
 #include <diagnostic_updater/diagnostic_updater.h>
 #include "ros/ros.h"
 #include <sensor_msgs/Joy.h>
+#include <sensor_msgs/JoyFeedbackArray.h>
+#include <linux/input.h>
+#include <unistd.h>
 
 
 ///\brief Opens, reads from and publishes joystick events
@@ -51,6 +54,7 @@ class Joystick
   bool default_trig_val_;
   std::string joy_dev_;
   std::string joy_dev_name_;
+  std::string joy_def_ff_;
   double deadzone_;
   double autorepeat_rate_;   // in Hz.  0 for no repeat.
   double coalesce_interval_; // Defaults to 100 Hz rate limit.
@@ -58,6 +62,8 @@ class Joystick
   int pub_count_;
   ros::Publisher pub_;
   double lastDiagTime_;
+  int ff_fd_;
+  struct ff_effect joy_effect_;
   
   diagnostic_updater::Updater diagnostic_;
   
@@ -139,9 +145,36 @@ class Joystick
   }
   
 public:
-  Joystick() : nh_(), diagnostic_()
+  Joystick() : nh_(), diagnostic_(), ff_fd_(-1)
   {}
-  
+
+  void set_feedback(const sensor_msgs::JoyFeedbackArray::ConstPtr& msg)
+  {
+    if (ff_fd_ == -1)
+      return;//we arent ready yet
+    
+    int size = msg->array.size();
+    for (int i = 0; i < size; i++)
+    {
+      //process each feedback
+      if (msg->array[i].type == 1)//TYPE_RUMBLE
+      {
+        //if id is zero, thats low freq, 1 is high
+        joy_effect_.direction = 0;//down
+        joy_effect_.type = FF_RUMBLE;
+		if (msg->array[i].id == 1)
+          joy_effect_.u.rumble.strong_magnitude = ((float)(1<<15))*msg->array[i].intensity;
+        else  
+          joy_effect_.u.rumble.weak_magnitude = ((float)(1<<15))*msg->array[i].intensity;
+        joy_effect_.replay.length = 100;
+        joy_effect_.replay.delay = 0;
+		
+        //upload the effect
+        int ret = ioctl(ff_fd_, EVIOCSFF, &joy_effect_);
+      }
+    }
+  }
+
   ///\brief Opens joystick port, reads from port and publishes while node is active
   int main(int argc, char **argv)
   {
@@ -151,7 +184,9 @@ class Joystick
     // Parameters
     ros::NodeHandle nh_param("~");
     pub_ = nh_.advertise<sensor_msgs::Joy>("joy", 1);
+	ros::Subscriber sub = nh_.subscribe("joy/set_feedback", 10, &Joystick::set_feedback, this);
     nh_param.param<std::string>("dev", joy_dev_, "/dev/input/js0");
+    nh_param.param<std::string>("dev_ff", joy_def_ff_, "/dev/input/event15");
     nh_param.param<std::string>("dev_name", joy_dev_name_, "");
     nh_param.param<double>("deadzone", deadzone_, 0.05);
     nh_param.param<double>("autorepeat_rate", autorepeat_rate_, 0);
@@ -251,6 +286,30 @@ class Joystick
         sleep(1.0);
         diagnostic_.update();
       }
+
+      ff_fd_ = open(joy_def_ff_.c_str(), O_RDWR);
+	
+      /* Set the gain of the device*/
+      int gain = 75;           /* between 0 and 100 */
+      struct input_event ie;      /* structure used to communicate with the driver */
+
+      ie.type = EV_FF;
+      ie.code = FF_GAIN;
+      ie.value = 0xFFFFUL * gain / 100;
+
+      if (write(ff_fd_, &ie, sizeof(ie)) == -1)
+        perror("set gain");
+
+      joy_effect_.id = -1;//0;
+      joy_effect_.direction = 0;//down
+      joy_effect_.type = FF_RUMBLE;
+      joy_effect_.u.rumble.strong_magnitude = 0;
+      joy_effect_.u.rumble.weak_magnitude = 0;
+      joy_effect_.replay.length = 100;
+      joy_effect_.replay.delay = 0;
+		
+      //upload the effect
+      int ret = ioctl(ff_fd_, EVIOCSFF, &joy_effect_);
       
       ROS_INFO("Opened joystick: %s. deadzone_: %f.", joy_dev_.c_str(), deadzone_);
       open_ = true;
@@ -284,6 +343,13 @@ class Joystick
           continue;
           //break; // Joystick is probably closed. Not sure if this case is useful.
         }
+
+        //play the rumble effect (can probably do this at lower rate later)
+        struct input_event start;
+		start.type = EV_FF;
+		start.code = joy_effect_.id;
+		start.value = 3;
+		write(ff_fd_, (const void*) &start, sizeof(start));
         
         if (FD_ISSET(joy_fd, &set))
         {