KITHandUnit.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::KITHandUnit
 * @author     Stefan Reither ( stefan dot reither at kit dot edu )
 * @date       2019
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "KITHandUnit.h"
#include <ArmarXCore/core/ArmarXManager.h>
#include <ArmarXGui/libraries/RemoteGui/WidgetBuilder.h>
 
#include <algorithm>
 
using namespace KITHand;
 
namespace armarx
{
    KITHandUnitPropertyDefinitions::KITHandUnitPropertyDefinitions(std::string prefix) :
        armarx::HandUnitPropertyDefinitions(prefix)
    {
        defineOptionalProperty<KITHandCommunicationDriver::ScanMode>("ConnectionType", KITHandCommunicationDriver::ScanMode::Bluetooth, "Type of the connection to the hand, either Bluetooth or Serial")
        .map("Bluetooth", KITHandCommunicationDriver::ScanMode::Bluetooth)
        .map("Serial", KITHandCommunicationDriver::ScanMode::Serial);
        defineRequiredProperty<std::string>("MAC-Address", "MAC-Address of the hand to connect to.");
        defineOptionalProperty<bool>("AutomaticReconnectActive", false, "Whether the hand unit should try to reconnect after the connection is lost.");
        defineOptionalProperty<bool>("ScanUntilHandFound", true, "Wheather to keep scanning until the hand with the given MAC-Address is found.");
        defineOptionalProperty<int>("ScanTimeout", 5, "Timeout for scanning repeatedly.");
    }
 
 
    std::string KITHandUnit::getDefaultName() const
    {
        return "KITHandUnit";
    }
 
    void KITHandUnit::onInitHandUnit()
    {
        //addShapeName("Open"); //is added by something else already
        addShapeName("Close");
        addShapeName("G0");
        addShapeName("G1");
        addShapeName("G2");
        addShapeName("G3");
        addShapeName("G4");
        addShapeName("G5");
        addShapeName("G6");
        addShapeName("G7");
        addShapeName("G8");
 
        _driver = std::make_unique<KITHandCommunicationDriver>();
        _driver->registerConnectionStateChangedCallback(std::bind(&KITHandUnit::connectionStateChangedCallback, this, std::placeholders::_1));
    }
 
    void KITHandUnit::onStartHandUnit()
    {
        std::string macAddress = getProperty<std::string>("MAC-Address");
        KITHandCommunicationDriver::ScanMode mode = getProperty<KITHandCommunicationDriver::ScanMode>("ConnectionType").getValue();
        bool found = false;
        auto start = std::chrono::system_clock::now();
        do
        {
            std::vector<HandDevice> devices = _driver->scanForDevices(mode);
            for (HandDevice& d : devices)
            {
                if (mode == KITHandCommunicationDriver::ScanMode::Bluetooth)
                {
                    ARMARX_INFO << "Found device with MAC-Address: " << d.macAdress;
                }
                else if (mode == KITHandCommunicationDriver::ScanMode::Serial)
                {
                    ARMARX_INFO << "Found device at serial port: " << d.serialDeviceFile;
                }
                else
                {
                    ARMARX_INFO << "Found hand"; //TODO
                }
                if (d.macAdress == macAddress || (mode == KITHandCommunicationDriver::ScanMode::Serial))
                {
                    d.hardwareTarget = mode == KITHandCommunicationDriver::ScanMode::Bluetooth ? KITHand::HardwareTarget::Bluetooth : KITHand::HardwareTarget::Serial;
                    _driver->connect(d);
                    while (!_driver->connected())
                    {
                        std::this_thread::sleep_for(std::chrono::milliseconds(100));
                    }
                    found = true;
 
                    //gui
                    createOrUpdateRemoteGuiTab(buildGui(), [this](RemoteGui::TabProxy & prx)
                    {
                        processGui(prx);
                    });
                    return;
                }
            }
        }
        while (getProperty<bool>("ScanUntilHandFound").getValue()
               && !found
               && std::chrono::duration_cast<std::chrono::seconds>(std::chrono::system_clock::now() - start).count() < getProperty<int>("ScanTimeout").getValue());
 
        if (mode == KITHandCommunicationDriver::ScanMode::Bluetooth)
        {
            ARMARX_WARNING << "Could not find hand with the given MAC-Address: " << macAddress << " Shutting down this KITHandUnit.";
        }
        else
        {
            ARMARX_WARNING << "Could not find hand ove serial ports. Shutting down this KITHHandUnit";
        }
        getArmarXManager()->asyncShutdown();
    }
 
    void KITHandUnit::onExitHandUnit()
    {
        ARMARX_IMPORTANT << "Calling driver disconnect";
        _driver->disconnect();
        _driver.reset();
    }
 
    void KITHandUnit::setShape(const std::string& shapeName, const Ice::Current&)
    {
        if (std::regex_match(shapeName, std::regex{"[gG](0|[1-9][0-9]*)"}) && _driver->getCurrentConnectedDevice()->abilities.receivesAdditionalGraspCommands)
        {
            _driver->sendGrasp(std::stoul(shapeName.substr(1)));
        }
        else if (shapeName == "Open")
        {
            _driver->sendGrasp(0);
        }
        else if (shapeName == "Close")
        {
            _driver->sendGrasp(1);
        }
        else if (!_shapes.count(shapeName))
        {
            ARMARX_WARNING << "Unknown shape name '" << shapeName
                           << "'\nKnown shapes: " << _shapes;
        }
        else
        {
            setJointAngles(_shapes.at(shapeName));
        }
    }
 
    void KITHandUnit::setJointAngles(const NameValueMap& targetJointAngles, const Ice::Current&)
    {
        ARMARX_CHECK_NOT_NULL(_driver);
 
        for (const std::pair<std::string, float>& pair : targetJointAngles)
        {
            if (pair.first == "Fingers")
            {
                const std::uint64_t pos = std::clamp(
                                              static_cast<std::uint64_t>(pair.second * KITHand::ControlOptions::maxPosFingers),
                                              static_cast<std::uint64_t>(0),
                                              KITHand::ControlOptions::maxPosFingers);
                ARMARX_DEBUG << deactivateSpam(1, std::to_string(pos)) << "set fingers " << pos;
                _driver->sendFingersPosition(pos);
            }
            else if (pair.first == "Thumb")
            {
                const std::uint64_t pos = std::clamp(
                                              static_cast<std::uint64_t>(pair.second * KITHand::ControlOptions::maxPosThumb),
                                              static_cast<std::uint64_t>(0),
                                              KITHand::ControlOptions::maxPosThumb);
                ARMARX_DEBUG << deactivateSpam(1, std::to_string(pos)) << "set thumb " << pos;
                _driver->sendThumbPosition(pos);
            }
            else
            {
                ARMARX_WARNING << "Invalid HandJointName '" << pair.first << "', ignoring.";
            }
            _driver->waitForCommunicationMedium();
        }
    }
 
    NameValueMap KITHandUnit::getCurrentJointValues(const Ice::Current&)
    {
        NameValueMap jointValues;
        jointValues["Fingers"] = _driver->getFingersPos() * 1.f / KITHand::ControlOptions::maxPosFingers;
        jointValues["Thumb"] = _driver->getThumbPos() * 1.f / KITHand::ControlOptions::maxPosThumb;
        return jointValues;
    }
 
    void KITHandUnit::addShape(const std::string& name, const std::map<std::string, float>& shape)
    {
        _shapes[name] = shape;
        addShapeName(name);
    }
 
    void KITHandUnit::addShapeName(const std::string& name)
    {
        Variant currentPreshape;
        currentPreshape.setString(name);
        shapeNames->addVariant(currentPreshape);
    }
 
 
    armarx::PropertyDefinitionsPtr KITHandUnit::createPropertyDefinitions()
    {
        return armarx::PropertyDefinitionsPtr(new KITHandUnitPropertyDefinitions(
                getConfigIdentifier()));
    }
 
    void KITHandUnit::connectionStateChangedCallback(const State state)
    {
        if (state == State::DeviceLost)
        {
            getArmarXManager()->asyncShutdown();
        }
    }
 
    RemoteGui::WidgetPtr KITHandUnit::buildGui()
    {
        return RemoteGui::makeSimpleGridLayout().cols(3)
               .addTextLabel("Fingers")
               .addChild(RemoteGui::makeFloatSpinBox("Fingers")
                         .min(0).max(1).value(0))
               .addChild(new RemoteGui::HSpacer)
 
               .addTextLabel("Thumb")
               .addChild(RemoteGui::makeFloatSpinBox("Thumb")
                         .min(0).max(1).value(0))
               .addChild(new RemoteGui::HSpacer)
 
               .addChild(RemoteGui::makeCheckBox("AutoSendValues").value(false).label("Auto send"))
               .addChild(RemoteGui::makeButton("SendValues").label("Send"))
               .addChild(new RemoteGui::HSpacer)
 
               .addChild(RemoteGui::makeLineEdit("Raw").value("M2,1000,1000,100000"))
               .addChild(RemoteGui::makeButton("SendRaw").label("Send Raw"))
               .addChild(new RemoteGui::HSpacer);
    }
 
    void KITHandUnit::processGui(RemoteGui::TabProxy& prx)
    {
        prx.receiveUpdates();
        if (prx.getValue<bool>("AutoSendValues").get() || prx.getButtonClicked("SendValues"))
        {
            const auto fingers = prx.getValue<float>("Fingers").get();
            const auto thumb = prx.getValue<float>("Thumb").get();
            ARMARX_INFO << "setting Fingers " << fingers << " and Thumb " << thumb;
            setJointAngles({{"Fingers", fingers}, {"Thumb", thumb}});
        }
        if (prx.getButtonClicked("SendRaw") && _driver)
        {
            _driver->sendRaw(prx.getValue<std::string>("Raw").get());
        }
    }
}