GamepadUnit.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    RobotAPI::ArmarXObjects::GamepadUnit
 * @author     Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
 * @date       2017
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "GamepadUnit.h"
 
#include "ArmarXCore/core/logging/Logging.h"
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
#include <ArmarXCore/observers/variant/TimestampVariant.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <linux/joystick.h>
 
using namespace armarx;
 
ARMARX_REGISTER_COMPONENT_EXECUTABLE(armarx::GamepadUnit, "GamepadUnit");
 
void
GamepadUnit::onInitComponent()
{
    ARMARX_TRACE;
    offeringTopic(getProperty<std::string>("GamepadTopicName").getValue());
    deviceName = getProperty<std::string>("GamepadDeviceName").getValue();
    deviceEventName = getProperty<std::string>("GamepadForceFeedbackName").getValue();
    readTask = new RunningTask<GamepadUnit>(this, &GamepadUnit::run, "GamepadUnit");
}
 
void
GamepadUnit::onConnectComponent()
{
    ARMARX_TRACE;
    topicPrx =
        getTopic<GamepadUnitListenerPrx>(getProperty<std::string>("GamepadTopicName").getValue());
    ARMARX_TRACE;
    sendTask = new SimplePeriodicTask<>(
        [&]
        {
            ARMARX_TRACE;
            std::unique_lock lock(mutex);
            if (!js.opened())
            {
                return;
            }
            ARMARX_TRACE;
            if (!dataTimestamp)
            {
                ARMARX_INFO << deactivateSpam(1) << "dataTimestamp is null, waiting for value";
                return;
            }
            ARMARX_CHECK_NOT_NULL(dataTimestamp);
            const IceUtil::Time age = IceUtil::Time::now() - dataTimestamp->toTime();
            if (age.toMilliSeconds() < getProperty<int>("PublishTimeout").getValue())
            {
                ARMARX_TRACE;
                ARMARX_CHECK_NOT_NULL(topicPrx) << "Topic proxy must not be null.";
                topicPrx->reportGamepadState(deviceName, js.name, data, dataTimestamp);
            }
            else
            {
                ARMARX_TRACE;
                ARMARX_INFO << deactivateSpam(100000, std::to_string(dataTimestamp->getTimestamp()))
                            << "No new signal from gamepad for " << age.toMilliSecondsDouble()
                            << " milliseconds. Not sending data. Timeout: "
                            << getProperty<int>("PublishTimeout").getValue() << " ms";
            }
        },
        30);
 
    sendTask->start();
    ARMARX_TRACE;
    openGamepadConnection();
}
 
void
GamepadUnit::vibrate(const ::Ice::Current&)
{
    ARMARX_INFO << "vibration!";
    js.executeEffect();
}
 
bool
GamepadUnit::openGamepadConnection()
{
    if (js.open(deviceName, deviceEventName))
    {
 
        ARMARX_TRACE;
        ARMARX_INFO << "opened a gamepad named " << js.name << " with " << js.numberOfAxis
                    << " axis and " << js.numberOfButtons << " buttons.";
        if (js.numberOfAxis == 8 && js.numberOfButtons == 11)
        {
            ARMARX_TRACE;
            readTask->start();
        }
        else
        {
            ARMARX_TRACE;
            ARMARX_WARNING << "this is not our trusty logitech gamepad.";
            js.close();
            return false;
        }
    }
    else
    {
        ARMARX_TRACE;
        ARMARX_WARNING << "Could not open gamepad device " << deviceName;
        js.close();
        return false;
    }
    return true;
}
 
void
GamepadUnit::run()
{
    ARMARX_TRACE;
    while (readTask->isRunning())
    {
        ARMARX_TRACE;
        if (!js.pollEvent())
        {
            ARMARX_TRACE;
            ARMARX_WARNING << "failed to read gamepad data - trying to reconnect";
            js.close();
            sleep(1);
            while (readTask->isRunning() && !openGamepadConnection())
            {
                sleep(1);
            }
        }
        ARMARX_TRACE;
        std::unique_lock lock(mutex);
        IceUtil::Time now = IceUtil::Time::now();
        dataTimestamp = new TimestampVariant(now);
 
        //mapping found with command line tool jstest <device file>
 
        float axisFactor = 1.0f / 32768.f;
 
        data.leftStickY = js.axis[0] * axisFactor;
        data.leftStickX = js.axis[1] * axisFactor;
        data.rightStickX = js.axis[3] * axisFactor;
        data.rightStickY = js.axis[4] * axisFactor;
        data.dPadX = js.axis[7] * axisFactor;
        data.dPadY = js.axis[6] * axisFactor;
        data.leftTrigger = js.axis[2] * axisFactor;
        data.rightTrigger = js.axis[5] * axisFactor;
        ARMARX_TRACE;
 
        data.leftButton = js.buttonsPressed[4];
        data.rightButton = js.buttonsPressed[5];
        data.backButton = js.buttonsPressed[6];
        data.startButton = js.buttonsPressed[7];
        data.xButton = js.buttonsPressed[2];
        data.yButton = js.buttonsPressed[3];
        data.aButton = js.buttonsPressed[0];
        data.bButton = js.buttonsPressed[1];
        data.theMiddleButton = js.buttonsPressed[8];
        data.leftStickButton = js.buttonsPressed[9];
        data.rightStickButton = js.buttonsPressed[10];
        ARMARX_TRACE;
 
        ARMARX_VERBOSE << "left x (integer): " << js.axis[0]
                       << " left x (float): " << data.leftStickX
                       << " right trigger: " << data.rightTrigger;
 
        //usleep(1000); // 10ms
    }
}
 
void
GamepadUnit::onDisconnectComponent()
{
    ARMARX_TRACE;
    if (sendTask)
    {
        ARMARX_TRACE;
        sendTask->stop();
    }
    if (readTask)
    {
        ARMARX_TRACE;
        readTask->stop();
    }
}
 
void
GamepadUnit::onExitComponent()
{
}
 
armarx::PropertyDefinitionsPtr
GamepadUnit::createPropertyDefinitions()
{
    return armarx::PropertyDefinitionsPtr(
        new GamepadUnitPropertyDefinitions(getConfigIdentifier()));
}