diff --git a/controldev.orogen b/controldev.orogen index bfa3a4c..2a6c306 100644 --- a/controldev.orogen +++ b/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 diff --git a/tasks/GenericTask.cpp b/tasks/GenericTask.cpp index 9f2ee58..42a7ad7 100644 --- a/tasks/GenericTask.cpp +++ b/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(); } - diff --git a/tasks/GenericTask.hpp b/tasks/GenericTask.hpp index ec1fb54..51f34f0 100644 --- a/tasks/GenericTask.hpp +++ b/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 axisHandles; std::vector 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(); - - - }; } diff --git a/tasks/JoystickTask.cpp b/tasks/JoystickTask.cpp index ac08d76..9205338 100644 --- a/tasks/JoystickTask.cpp +++ b/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,25 +25,91 @@ 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(); + 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) { @@ -51,31 +117,42 @@ bool JoystickTask::updateRawCommand(RawCommand& rcmd) { 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; } - diff --git a/tasks/JoystickTask.hpp b/tasks/JoystickTask.hpp index 46a5b13..6f790bb 100644 --- a/tasks/JoystickTask.hpp +++ b/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