Allow joystick disconnection and reconnection instead of going into exception

controldev.orogen

@@ -27,6 +27,8 @@ end
 task_context "JoystickTask", subclasses: 'GenericTask' do
     default_activity :fd_driven
 
+    runtime_states "CONNECTED", "NOT_CONNECTED"
+
     # Path to the joystick device
     property "device", "/std/string", "/dev/input/js0"
 end

tasks/GenericTask.cpp

@@ -53,8 +53,7 @@ bool GenericTask::startHook()
 void GenericTask::updateHook()
 {
     GenericTaskBase::updateHook();
-
-
+
     RawCommand rcmd;
     updateRawCommand(rcmd);
 
@@ -63,13 +62,13 @@ void GenericTask::updateHook()
         std::cout << "Error, more scale factors than axes defined : Nr axes " << rcmd.axisValue.size() << " size of axis scales " << axisScales.size() << std::endl;
         exception();
     }
-    
+
     RawCommand rscaled = rcmd;
     for(size_t i = 0 ; i < axisScales.size(); i++)
     {
         rscaled.axisValue[i] *= axisScales[i];
     }
-    
+
     _raw_command.write(rscaled);
 
     for(size_t i = 0 ; i < axisHandles.size(); i++)
@@ -95,4 +94,3 @@ void GenericTask::stopHook()
     if(activity)
         activity->clearAllWatches();
 }
-

tasks/GenericTask.hpp

@@ -8,12 +8,12 @@
 namespace controldev {
     class GenericTask : public GenericTaskBase
     {
-	friend class GenericTaskBase;
+        friend class GenericTaskBase;
     protected:
         virtual bool updateRawCommand(RawCommand& rcmd) = 0;
 
         virtual int getFileDescriptor() = 0;
-        
+
         class AxisPortHandle
         {
         public:
@@ -23,11 +23,12 @@ namespace controldev {
 
         std::vector<AxisPortHandle> axisHandles;
         std::vector<double> axisScales;
+
     public:
         GenericTask(std::string const& name = "controldev::GenericTask");
         GenericTask(std::string const& name, RTT::ExecutionEngine* engine);
 
-	virtual ~GenericTask();
+        virtual ~GenericTask();
 
         /** This hook is called by Orocos when the state machine transitions
          * from PreOperational to Stopped. If it returns false, then the
@@ -90,9 +91,6 @@ namespace controldev {
          */
         // void cleanupHook();
 
-
-
-
     };
 }
 

tasks/JoystickTask.cpp

@@ -7,12 +7,12 @@
 using namespace controldev;
 
 JoystickTask::JoystickTask(std::string const& name)
-    : JoystickTaskBase(name), joystick(new controldev::Joystick())
+    : JoystickTaskBase(name), joystick(new controldev::Joystick()), device_connected_(false)
 {
 }
 
 JoystickTask::JoystickTask(std::string const& name, RTT::ExecutionEngine* engine)
-    : JoystickTaskBase(name, engine), joystick(new controldev::Joystick())
+    : JoystickTaskBase(name, engine), joystick(new controldev::Joystick()), device_connected_(false)
 {
 }
 
@@ -25,57 +25,134 @@ JoystickTask::~JoystickTask()
 // hooks defined by Orocos::RTT. See JoystickTask.hpp for more detailed
 // documentation about them.
 
+int JoystickTask::getFileDescriptor()
+{
+    return joystick->getFileDescriptor();
+}
+
 bool JoystickTask::configureHook()
 {
     if (! JoystickTaskBase::configureHook())
         return false;
-
+
+    connectDevice();
+    return true;
+}
+
+void JoystickTask::connectDevice(bool recover)
+{
     // Try to connect the Joystick
-    if (!joystick->init(_device.value()))
+    if (recover)
     {
-        std::cerr << "Warning: Unable to open Joystick device "
-            << _device.value() << std::endl;
-	return false;
+        if (not recoverConnection())
+        {
+            return;
+        }
     }
 
-    return true;
+    if (joystick->init(_device.value()))
+    {
+        if (! device_connected_)
+        {
+            device_connected_ = true;
+            std::cout << "INFO: Device " << _device.value() << " connected" << std::endl;
+        }
+    } else {
+        if (device_connected_)
+        {
+            std::cerr << "Warning: Unable to open Joystick device "
+                << _device.value() << std::endl;
+            device_connected_ = false;
+        }
+    }
 }
 
-int JoystickTask::getFileDescriptor()
+bool JoystickTask::recoverConnection()
 {
-    return joystick->getFileDescriptor();
+    delete joystick;
+    // from generic stop hook
+    RTT::extras::FileDescriptorActivity* activity =
+        getActivity<RTT::extras::FileDescriptorActivity>();
+    if(activity)
+        activity->clearAllWatches();
+
+    joystick = new controldev::Joystick();
+    joystick->init(_device.value());
+    int fdID = joystick->getFileDescriptor();
+
+    // from generic start hook
+    if (activity && fdID != -1)
+    {
+        activity->watch(getFileDescriptor());
+        //get trigger a least every 25 ms
+        activity->setTimeout(25);
+        return true;
+    } else {
+        return false;
+    }
+}
+
+void JoystickTask::updateHook()
+{
+    if (device_connected_)
+    {
+        state(CONNECTED);
+        JoystickTaskBase::updateHook();
+        writeCommand();
+    } else {
+        state(NOT_CONNECTED);
+        connectDevice(true);
+    }
+}
+
+void JoystickTask::writeCommand()
+{
+    RawCommand rcmd;
+    updateRawCommand(rcmd);
+    _raw_command.write(rcmd);
 }
 
 bool JoystickTask::updateRawCommand(RawCommand& rcmd) {
     // New data available at the Joystick device
     while(joystick->updateState())
     {
     }
-    
+
     rcmd.deviceIdentifier= joystick->getName();
-
     rcmd.axisValue = joystick->getAxes();
-
-    size_t buttonCount = joystick->getNrButtons();
+    if (not checkAxisValues(rcmd) )
+    {
+        throw std::runtime_error("Invalid axis values. Try to forget device and reconnect");
+    }
 
-    // Set button bit list
-    for (size_t i = 0; i < buttonCount; i++)
+    size_t buttonCount = joystick->getNrButtons();
+    try
     {
-        rcmd.buttonValue.push_back(joystick->getButtonPressed(i));
+        // Set button bit list
+        for (size_t i = 0; i < buttonCount; i++)
+        {
+            rcmd.buttonValue.push_back(joystick->getButtonPressed(i));
+        }
+    } catch (const std::runtime_error& e) {
+        std::cerr << "Warning: Caught runtime error. Lost connection to device." << std::endl;
+        device_connected_ = false;
+        state(NOT_CONNECTED);
+        return false;
     }
 
     rcmd.time = base::Time::now();
-
     return true;
 }
 
-
-void JoystickTask::updateHook()
+bool JoystickTask::checkAxisValues(const RawCommand& rmcd)
 {
-    JoystickTaskBase::updateHook();
-
-    RawCommand rcmd;
-    updateRawCommand(rcmd);
-    _raw_command.write(rcmd);
+    unsigned int counter = 0;    
+    for (const auto & value : rmcd.axisValue)
+    {
+        if (value == -1)
+        {
+            counter++;
+        }
+    }
+    return counter < 5;
 }
-

tasks/JoystickTask.hpp

@@ -13,14 +13,20 @@ namespace controldev {
 	friend class JoystickTaskBase;
     protected:
         Joystick *joystick;
+        bool device_connected_;
+
+        virtual int getFileDescriptor() override;
+        virtual bool updateRawCommand(RawCommand& rcmd) override;
+        void connectDevice(bool recover = false);
+        bool recoverConnection();
+        void writeCommand();
+        bool checkAxisValues(const RawCommand& rcmd);
 
-        virtual bool updateRawCommand(RawCommand& rcmd);
-        virtual int getFileDescriptor();
     public:
         JoystickTask(std::string const& name = "controldev::JoystickTask");
         JoystickTask(std::string const& name, RTT::ExecutionEngine* engine);
 
-	~JoystickTask();
+        ~JoystickTask();
 
         /** This hook is called by Orocos when the state machine transitions
          * from PreOperational to Stopped. If it returns false, then the