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 <algorithm>
 
#include <ArmarXCore/core/ArmarXManager.h>
 
#include <ArmarXGui/libraries/RemoteGui/WidgetBuilder.h>
 
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());
        }
    }
} // namespace armarx