GamepadControlUnit.cpp

/*
 * 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::GamepadControlUnit
 * @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 "GamepadControlUnit.h"

#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/core/time/Duration.h>
#include <ArmarXCore/core/time/ice_conversions.h>
#include <ArmarXCore/observers/variant/TimestampVariant.h>

namespace armarx
{
    void
    GamepadControlUnit::onInitComponent()
    {
        ARMARX_INFO << "oninit GamepadControlUnit";
        usingProxy(getProperty<std::string>("PlatformUnitName").getValue());
        usingTopic(getProperty<std::string>("GamepadTopicName").getValue());
        usingProxy("EmergencyStopMaster");

        scaleX = getProperty<float>("ScaleX").getValue();
        scaleY = getProperty<float>("ScaleY").getValue();
        scaleRotation = getProperty<float>("ScaleAngle").getValue();
        ARMARX_INFO << "oninit GamepadControlUnit end";
    }

    void
    GamepadControlUnit::onConnectComponent()
    {
        ARMARX_INFO << "onConnect GamepadControlUnit";
        platformUnitPrx = getProxy<PlatformUnitInterfacePrx>(
            getProperty<std::string>("PlatformUnitName").getValue());
        emergencyStop = getProxy<EmergencyStopMasterInterfacePrx>("EmergencyStopMaster");


        if (enableHeartBeat)
        {
            this->heartbeatPlugin->signUp(armarx::core::time::Duration::MilliSeconds(1000),
                                          armarx::core::time::Duration::MilliSeconds(1500),
                                          {"Gamepad"},
                                          "The GamepadControlUnit");
        }
    }

    void
    GamepadControlUnit::onDisconnectComponent()
    {
    }

    void
    GamepadControlUnit::onExitComponent()
    {
        ARMARX_INFO << "exit GamepadControlUnit";
    }

    armarx::PropertyDefinitionsPtr
    GamepadControlUnit::createPropertyDefinitions()
    {
        auto defs = armarx::PropertyDefinitionsPtr(
            new GamepadControlUnitPropertyDefinitions(getConfigIdentifier()));
        defs->optional(enableHeartBeat,
                       "EnableHeartBeat",
                       "Flag to enable send a heart beat to the robot healh topic");
        return defs;
    }

    void
    GamepadControlUnit::reportGamepadState(const std::string& device,
                                           const std::string& name,
                                           const GamepadData& data,
                                           const TimestampBasePtr& timestamp,
                                           const Ice::Current& c)
    {


        // struct GamepadData {
        //     float leftStickX;
        //     float leftStickY;
        //     float rightStickX;
        //     float rightStickY;
        //     float dPadX;
        //     float dPadY;
        //     float leftTrigger;
        //     float rightTrigger;
        //
        //     bool leftButton;
        //     bool rightButton;
        //     bool backButton;
        //     bool startButton;
        //     bool xButton;
        //     bool yButton;
        //     bool aButton;
        //     bool bButton;
        //     bool theMiddleButton;
        //     bool leftStickButton;
        //     bool rightStickButton;
        //
        // };


        if (data.leftTrigger > 0)
        {
            emergencyStop->setEmergencyStopState(EmergencyStopState::eEmergencyStopActive);
            return;
        }
        else if (data.startButton)
        {
            emergencyStop->setEmergencyStopState(EmergencyStopState::eEmergencyStopInactive);
        }
        //scales are for the robot axis
        if (data.rightTrigger > 0)
        {
            platformUnitPrx->move(data.leftStickY * scaleX,
                                  data.leftStickX * scaleY,
                                  data.rightStickX * scaleRotation);
        }
        else
        {
            platformUnitPrx->move(0, 0, 0);
        }


        if (data.leftButton)
        {

            if (leftHandTime <= 0.0)
            {
                leftHandTime = IceUtil::Time::now().toMicroSeconds();
            }
            else if ((IceUtil::Time::now().toMicroSeconds() - leftHandTime) > 1000 * 1000)
            {

                HandUnitInterfacePrx handUnit = getProxy<HandUnitInterfacePrx>("LeftHandUnit");
                if (handUnit)
                {
                    std::string shapeName = (leftHandOpen) ? "Close" : "Open";
                    handUnit->setShape(shapeName);
                    leftHandOpen = !leftHandOpen;
                    leftHandTime = 0.0;
                }
            }
        }
        else
        {
            leftHandTime = 0.0;
        }

        if (data.rightButton)
        {

            if (rightHandTime <= 0.0)
            {
                rightHandTime = IceUtil::Time::now().toMicroSeconds();
            }
            else if ((IceUtil::Time::now().toMicroSeconds() - rightHandTime) > 1000 * 1000)
            {
                HandUnitInterfacePrx handUnit = getProxy<HandUnitInterfacePrx>("RightHandUnit");
                if (handUnit)
                {
                    std::string shapeName = (rightHandOpen) ? "Close" : "Open";
                    handUnit->setShape(shapeName);
                    rightHandOpen = !rightHandOpen;
                    rightHandTime = 0.0;
                }
            }
        }
        else
        {
            rightHandTime = 0.0;
        }

        if (enableHeartBeat)
        {
            heartbeatPlugin->heartbeat();
        }

        //ARMARX_INFO << "sending targets" << data.leftStickX* scaleX << " " << data.leftStickY* scaleY << " " << data.rightStickX* scaleRotation;
    }

    GamepadControlUnit::GamepadControlUnit()
    {
        addPlugin(heartbeatPlugin);
    }
} // namespace armarx