ATINetFTUnit.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * 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    VisionX::ArmarXObjects::ATINetFTUnit
 * @author     Jonas Rauber ( kit at jonasrauber dot de )
 * @date       2014
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "ATINetFTUnit.h"
 
#include <inttypes.h>
#include <stdio.h>
#include <stdlib.h>
 
#include <iostream>
#include <string>
#include <vector>
 
#include <arpa/inet.h>
#include <netdb.h>
#include <sys/socket.h>
 
namespace armarx
{
    void
    ATINetFTUnit::onInitComponent()
    {
    }
 
    void
    ATINetFTUnit::onConnectComponent()
    {
        /*
         * Read ATINetFT hostname and port from properties
         * std::string receiverHostName("192.168.1.1:49152");
         */
 
        std::string ATINetFTReceiverHostName =
            getProperty<std::string>("ATINetFTReceiveHostName").getValue();
        std::string captureName = getProperty<std::string>("CaptureName").getValue();
        ARMARX_INFO << "Capture Name " << captureName;
        size_t pos = captureName.find("0");
        captureNamePrefix = captureName.substr(0, pos);
        ARMARX_INFO << "Capture Name Prefix " << captureNamePrefix;
        std::string captureNameSuffix = captureName.substr(pos, captureName.size());
        ARMARX_INFO << "Capture Name Suffix " << captureNameSuffix;
        nPaddingZeros = captureNameSuffix.size();
        captureNumber = std::stoi(captureNameSuffix);
        establishATINetFTReceiveHostConnection(ATINetFTReceiverHostName);
        ftDataSize = 6;
        sendPacketSize = 8;
        receivePacketSize = 36;
        recordingStopped = true;
        recordingCompleted = true;
        recordingStarted = false;
 
        /*
         * Connect to ATINetFT
         */
        /*if (ATINetFT.Connect(hostName).Result != Result::Success)
        {
            ARMARX_ERROR << "Could not connect to ATINetFT at " << hostName << flush;
            return;
        }
        else
        {
            ARMARX_IMPORTANT << "Connected to ATINetFT at " << hostName << flush;
        }*/
 
        /*
         * Enable Marker Data
         */
        /*Result::Enum result = ATINetFT.EnableMarkerData().Result;
 
        if (result != Result::Success)
        {
            ARMARX_ERROR << "ATINetFT EnableMarkerData() returned error code " << result << flush;
            return;
        }*/
    }
 
    void
    ATINetFTUnit::onDisconnectComponent()
    {
        ARMARX_IMPORTANT << "Disconnecting from ATINetFT" << flush;
    }
 
    void
    ATINetFTUnit::onExitComponent()
    {
    }
 
    void
    ATINetFTUnit::periodicExec()
    {
        if (recordingStarted)
 
 
        {
            //ISO C++ forbids variable length array [-Werror=vla] => use a vector (the array will be on the heap anyways)
            std::vector<float> ftdata(ftDataSize);
            std::vector<char> receivePacketContent(receivePacketSize);
 
            if (!receivePacket(receivePacketContent.data()))
            {
                ARMARX_INFO << "recvfrom() failed  :";
            }
            else
            {
                convertToFTValues(receivePacketContent.data(), ftdata.data(), ftdata.size());
                writeFTValuesToFile(ftdata.data(), ftdata.size());
                sampleIndex++;
            }
        }
    }
 
    PropertyDefinitionsPtr
    ATINetFTUnit::createPropertyDefinitions()
    {
        return PropertyDefinitionsPtr(new ATINetFTUnitPropertyDefinitions(getConfigIdentifier()));
    }
 
    void
    ATINetFTUnit::startRecording(const std::string& customName, const Ice::Current& c)
    {
        if (remoteTriggerEnabled)
        {
            if (recordingCompleted)
            {
                int bpSize = sendPacketSize;
                //ISO C++ forbids variable length array [-Werror=vla] => use a vector (the array will be on the heap anyways)
                std::vector<char> bytePacket(bpSize);
                *(uint16_t*)&bytePacket[0] = htons(0x1234); /* standard header. */
                *(uint16_t*)&bytePacket[2] = htons(2); /* per table 9.1 in Net F/T user manual. */
                *(uint32_t*)&bytePacket[4] = htonl(0); /* see section 9.1 in Net F/T user manual. */
                recordingStarted = sendPacket(bytePacket.data(), bytePacket.size());
 
                if (recordingStarted)
                {
                    recordingStopped = false;
                    recordingCompleted = false;
                    //TODO build filename captureNamePrefic+customName
                    recordingFile.open(captureNamePrefix);
                    sampleIndex = 0;
                }
            }
        }
    }
 
    void
    ATINetFTUnit::stopRecording(const Ice::Current& c)
    {
        if (remoteTriggerEnabled)
        {
            int bpSize = 8;
            //ISO C++ forbids variable length array [-Werror=vla] => use a vector (the array will be on the heap anyways)
            std::vector<char> bytePacket(bpSize);
            *(uint16_t*)&bytePacket[0] = htons(0x1234); /* standard header. */
            *(uint16_t*)&bytePacket[2] = htons(0); /* per table 9.1 in Net F/T user manual. */
            *(uint32_t*)&bytePacket[4] = htonl(0); /* see section 9.1 in Net F/T user manual. */
 
            recordingStopped = sendPacket(bytePacket.data(), bytePacket.size());
            if (recordingStopped)
            {
                recordingFile.close();
                recordingStarted = false;
            }
        }
    }
 
    bool
    ATINetFTUnit::sendPacket(char* bytePacket, int bpSize)
    {
        if (remoteTriggerEnabled)
        {
            if (sendto(senderSocket,
                       bytePacket,
                       bpSize,
                       0,
                       (struct sockaddr*)&receiveHostAddr,
                       sizeof(receiveHostAddr)) != bpSize)
            {
 
                return false;
            }
            return true;
        }
        return false;
    }
 
    bool
    ATINetFTUnit::receivePacket(char* receiveBuf)
    {
 
        int test = sizeof(receiveHostAddr);
 
        int receiveint = recvfrom(senderSocket,
                                  receiveBuf,
                                  receivePacketSize,
                                  0,
                                  (struct sockaddr*)&receiveHostAddr,
                                  ((socklen_t*)&test));
 
        if (receiveint < 0)
        {
 
            return false;
        }
 
 
        return true;
    }
 
    void
    ATINetFTUnit::convertToFTValues(char* receiveBuf, float* ftdata, int ftdataSize)
    {
        //uint32_t rdt_sequence = ntohl(*(uint32_t*)&receiveBuf[0]); //was an unused variable
        //uint32_t ft_sequence = ntohl(*(uint32_t*)&receiveBuf[4]); //was an unused variable
        //uint32_t status = ntohl(*(uint32_t*)&receiveBuf[8]); //was an unused variable
 
        for (int i = 0; i < ftdataSize; i++)
        {
            ftdata[i] = float(int32_t(ntohl(*(uint32_t*)&receiveBuf[12 + i * 4]))) / 1000000.0f;
        }
    }
 
    void
    ATINetFTUnit::writeFTValuesToFile(float* ftdata, int ftdataSize)
    {
        recordingFile << sampleIndex << " ";
        for (int i = 0; i < ftdataSize; i++)
        {
            recordingFile << ftdata[i] << " ";
        }
        recordingFile << "\n";
    }
 
    void
    ATINetFTUnit::establishATINetFTReceiveHostConnection(std::string receiverHostName)
    {
        remoteTriggerEnabled = false;
 
        //Construct the server sockaddr_ structure
        memset(&senderHostAddr, 0, sizeof(senderHostAddr));
        senderHostAddr.sin_family = AF_INET;
        senderHostAddr.sin_addr.s_addr = htonl(INADDR_ANY);
        senderHostAddr.sin_port = htons(0);
 
        //Create the socket
        if ((senderSocket = socket(AF_INET, SOCK_DGRAM, 0)) < 0)
        {
            return;
        }
 
        if (bind(senderSocket, (struct sockaddr*)&senderHostAddr, sizeof(senderHostAddr)) < 0)
        {
            return;
        }
 
        //Construct the server sockaddr_ structure
        memset(&receiveHostAddr, 0, sizeof(receiveHostAddr));
        size_t pos = receiverHostName.find(":");
        std::string hostname = receiverHostName.substr(0, pos);
 
        std::string hostport = receiverHostName.substr(pos + 1, receiverHostName.size());
 
        struct hostent* he;
        struct in_addr** addr_list;
        int i;
 
        if ((he = gethostbyname(hostname.c_str())) == NULL)
        {
            // get the host info
            herror("gethostbyname");
            return;
        }
 
        addr_list = (struct in_addr**)he->h_addr_list;
 
        for (i = 0; addr_list[i] != NULL; i++)
        {
 
            char ip[100];
            //Return the first one;
            strcpy(ip, inet_ntoa(*addr_list[i]));
 
            inet_pton(AF_INET, ip, &receiveHostAddr.sin_addr.s_addr);
 
            receiveHostAddr.sin_port = htons(atoi(hostport.c_str()));
            remoteTriggerEnabled = true;
            ARMARX_INFO << "Connection established to " << hostname << ":" << hostport;
            //if((receiverSocket=socket(AF_INET, SOCK_DGRAM, 0))<0) {
            //  return;
            //}
            //if(bind(receiverSocket,( struct sockaddr *) &receiveHostAddr, sizeof(receiveHostAddr))<0) {
            //  return;
            //}
 
            return;
        }
        return;
    }
} // namespace armarx