KeyboardTeleop.h

Go to the documentation of this file.

#ifndef KEYBOARDTELEOP_H_
#define KEYBOARDTELEOP_H_

#include <iostream>

#include <boost/thread.hpp>

#include <ros/ros.h>

#include <QWidget>
#include <QEvent>
#include <QKeyEvent>

using namespace std;

typedef boost::function<void (double, double)> VelocityCallback;

namespace mr_rqt {

class KeyboardTeleop : public QWidget {
Q_OBJECT

public:

        explicit KeyboardTeleop()
                : _updateThread(boost::bind(&KeyboardTeleop::updateThread, this))
        {
                _linear = 0;
                _angular = 0;
                _angularDown = false;
                _linearDown = false;
        }

        virtual ~KeyboardTeleop() { }

        void setCallback(VelocityCallback callback) {
                _callback = callback;
        }

        bool eventFilter(QObject* obj, QEvent *event)
        {
                if (event->type() == QEvent::KeyPress)
                {
                        QKeyEvent* keyEvent = static_cast<QKeyEvent*>(event);

                        switch (keyEvent->key()) {
                                case Qt::Key_Left:
                                        _angularDown = true;
                                        _angularDirectionSign = 1;
                                        break;
                                case Qt::Key_Right:
                                        _angularDown = true;
                                        _angularDirectionSign = -1;
                                        break;
                                case Qt::Key_Up:
                                        _linearDown = true;
                                        _linearDirectionSign = 1;
                                        break;
                                case Qt::Key_Down:
                                        _linearDown = true;
                                        _linearDirectionSign = -1;
                                        break;
                                default:
                                        break;
                        }



                        return true;
                }

                if (event->type() == QEvent::KeyRelease)
                {
                        QKeyEvent* keyEvent = static_cast<QKeyEvent*>(event);

                        switch (keyEvent->key()) {
                                case Qt::Key_Left:
                                case Qt::Key_Right:
                                        _angularDown = false;
                                        break;
                                case Qt::Key_Up:
                                case Qt::Key_Down:
                                        _linearDown = false;
                                        break;
                        }

                        return true;
                }

                return false;
        }

private:

        boost::thread _updateThread;
        double _linear;
        double _angular;
        volatile bool _linearDown;
        volatile bool _angularDown;
        int _linearDirectionSign;
        int _angularDirectionSign;
        VelocityCallback _callback;

        void updateThread() {
                while (ros::ok()) {

                        double linearStep = 0.2;
                        double angularStep = 0.2;

                        double decayFactor = 0.7;

                        if (_linearDown) {
                                _linear += linearStep * _linearDirectionSign;
                        } else {
                                _linear *= decayFactor;

                                if (fabs(_linear) < 0.001)
                                        _linear = 0;
                        }

                        if (_angularDown) {
                                _angular += angularStep * _angularDirectionSign;
                        } else {
                                _angular *= decayFactor;

                                if (fabs(_angular) < 0.001)
                                        _angular = 0;
                        }

                        _linear  = fmax(fmin(2.0, _linear), -2.0);
                        _angular = fmax(fmin(2.0, _angular), -2.0);

//                      ROS_WARN("Linear = %f, Angular = %f", _linear, _angular);

                        if (!_callback.empty())
                                _callback(_linear, _angular);

                        boost::this_thread::sleep(boost::posix_time::milliseconds(50));
                }
        }


};

}

#endif /* KEYBOARDTELEOP_H_ */