source/tools/NVRemoteConnection.cpp

Go to the documentation of this file.

/*
 * Copyright (c) 2007, 2008, 2009, University of Karlsruhe (for members see AUTHORS file)
 *
 * ISC License
 *
 * Permission to use, copy, modify, and/or distribute this software for any
 * purpose with or without fee is hereby granted, provided that the above
 * copyright notice and this permission notice appear in all copies.
 *
 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
 * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
 * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
 * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 *
 * Created by Manfred Kroehnert on 16.07.2008.
 */

#include "../include/IPSA/tools/NVRemoteConnection.h"
#include "../include/IPSA/network/TCPSocket.h"
#include "../include/IPSA/viewer/instructions/NVInstructionAnswerParsing.h"

#include <iostream>
#include <cassert>
#include <algorithm>
#include <iterator>


NVRemoteConnection::NVRemoteConnection(bool connectOnCreate /*false*/, const std::string& hostname /*"localhost"*/, unsigned int port /*12345*/)
: commandStream("")
{
                m_pSocket = new CTCPSocket(hostname.c_str(), port);
                if (connectOnCreate)
                                this->connect();
}


NVRemoteConnection::~NVRemoteConnection()
{
                delete m_pSocket; // also closes the socket
        m_pSocket = NULL;
}


void NVRemoteConnection::sendCommands()
{
                std::vector<float> answer(3, 0);
                std::string command = "";
        std::cout << "Sending commands " << std::endl;
                std::vector<std::string> jList = this->getJointList();
                std::cout << "JointList: " << std::endl;
                std::copy(jList.begin(), jList.end(),
                                  std::ostream_iterator<std::string>(std::cout, "\n"));


                this->scheduleOde();

                if (jList.empty())
                                return;

                int sleepTimeSeconds = 4;
                std::cout << "Sleeping for: " << sleepTimeSeconds << " seconds" << std::endl;
#ifdef WIN32
                Sleep(sleepTimeSeconds);
#else
                sleep(sleepTimeSeconds);
#endif
                // joint forces
                std::cout << "Joint Forces: " << std::endl;
                std::map<std::string, std::vector<float> > forceMap;
                forceMap = this->getJointForces(jList);
                for (unsigned int i = 0; i < forceMap.size(); i++)
                {
                                answer = forceMap[jList.at(i)];
                                std::copy(answer.begin(), answer.end(),
                                                  std::ostream_iterator<float>(std::cout, " "));
                                std::cout << std::endl;
                }

                // joint torques
                std::cout << "Joint Torques: " << std::endl;
                std::map<std::string, std::vector<float> > torqueMap;
                torqueMap = this->getJointForces(jList);
                for (unsigned int i = 0; i < torqueMap.size(); i++)
                {
                                answer = torqueMap[jList.at(i)];
                                std::copy(answer.begin(), answer.end(),
                                                                  std::ostream_iterator<float>(std::cout, " "));
                                std::cout << std::endl;
                }

                sleepTimeSeconds = 2;
#ifdef WIN32
                Sleep(sleepTimeSeconds);
#else
                sleep(sleepTimeSeconds);
#endif

                std::cout << "Joint Speed: " << std::endl;
                this->setJointSpeed(jList.front(), 5);

        std::cout << "done " << std::endl;
}


void NVRemoteConnection::scheduleOde(bool scheduleSimulation /*true*/)
{
                std::string scheduleCommand = "schedule\n";
                if (!scheduleSimulation)
                                scheduleCommand = "unschedule\n";

                m_pSocket->send(scheduleCommand);
}


std::vector<float>
NVRemoteConnection::getVectorForCommand(const std::string& commandName, const std::string&  nodeName, bool firstBody /*true*/)
{
                commandStream.str("");
                commandStream << commandName << " " << nodeName;
                if (!commandName.compare("getjointtorque") || !commandName.compare("getjointforce"))
                                commandStream << (firstBody ? " 1" : " 2");
                commandStream << std::endl;

                m_pSocket->send(commandStream.str());
                std::string answerString = "";
                std::vector<float> resultVector(3, 0);

                if (this->receiveAnswer(answerString))
                                convertAnswerToVector<float>(answerString, resultVector);

                return resultVector;
}


std::vector<float> NVRemoteConnection::getJointForce(const std::string& jointName, bool firstBody /*true*/)
{
                return getVectorForCommand("getjointforce", jointName, firstBody);
}


std::vector<float> NVRemoteConnection::getJointTorque(const std::string& jointName, bool firstBody /*true*/)
{
                return getVectorForCommand("getjointtorque", jointName, firstBody);
}


float NVRemoteConnection::getValueForCommand(const std::string& commandName, const std::string& nodeName)
{
                commandStream.str("");
                commandStream << commandName << " " << nodeName;
                commandStream << std::endl;

                m_pSocket->send(commandStream.str());
                static std::string AnswerString = "";
                float resultValue = 0.0f;

                if (this->receiveAnswer(AnswerString))
                                convertAnswerToType<float>(AnswerString, resultValue);

                return resultValue;
}


float NVRemoteConnection::getJointAngle(const std::string& jointName)
{
                return getValueForCommand("getjointangle", jointName);
}


float NVRemoteConnection::getJointAngleRate(const std::string& jointName)
{
                return getValueForCommand("getjointanglerate", jointName);
}


float NVRemoteConnection::getJointPosition(const std::string& jointName)
{
                return getValueForCommand("getjointposition", jointName);
}


float NVRemoteConnection::getJointPositionRate(const std::string& jointName)
{
                return getValueForCommand("getjointpositionrate", jointName);
}


std::map<std::string, std::vector<float> >
NVRemoteConnection::getMapForNodesAndCommand(const std::vector<std::string>& nodeList, std::vector<float> (NVRemoteConnection::*commandFunction)(const std::string&, bool))
{
                std::map<std::string, std::vector<float> > valueMap;

                if (!nodeList.empty())
                {
                                std::vector<std::string>::const_iterator iter = nodeList.begin();
                                while (iter != nodeList.end())
                                {
                                                // TODO second argument is mandatory but should be optional argument
                                                valueMap[(*iter)] = ((*this.*commandFunction)((*iter), true));
                                                iter++;
                                }

                }
                return valueMap;
}


std::map<std::string, float>
NVRemoteConnection::getMapForNodesAndCommand(const std::vector<std::string>& nodeList, float (NVRemoteConnection::*commandFunction)(const std::string&))
{
                std::map<std::string, float> valueMap;

                if (!nodeList.empty())
                {
                                std::vector<std::string>::const_iterator iter = nodeList.begin();
                                while (iter != nodeList.end())
                                {
                                                // TODO second argument is mandatory but should be optional argument
                                                valueMap[(*iter)] = ((*this.*commandFunction)(*iter));
                                                iter++;
                                }

                }
                return valueMap;
}


std::map<std::string, std::vector<float> >
NVRemoteConnection::getJointForces(const std::vector<std::string>& jointList)
{
                return getMapForNodesAndCommand(jointList, &NVRemoteConnection::getJointForce);
}


std::map<std::string, std::vector<float> >
NVRemoteConnection::getJointTorques(const std::vector<std::string>& jointList)
{
                return getMapForNodesAndCommand(jointList, &NVRemoteConnection::getJointTorque);
}


std::map<std::string, float> NVRemoteConnection::getJointAngles(const std::vector<std::string>& jointList)
{
                return getMapForNodesAndCommand(jointList, &NVRemoteConnection::getJointAngle);
}


std::map<std::string, float> NVRemoteConnection::getJointAngleRates(const std::vector<std::string>& jointList)
{
                return getMapForNodesAndCommand(jointList, &NVRemoteConnection::getJointAngleRate);
}


std::map<std::string, float> NVRemoteConnection::getJointPositions(const std::vector<std::string>& jointList)
{
                return getMapForNodesAndCommand(jointList, &NVRemoteConnection::getJointPosition);
}


std::map<std::string, float> NVRemoteConnection::getJointPositionRates(const std::vector<std::string>& jointList)
{
                return getMapForNodesAndCommand(jointList, &NVRemoteConnection::getJointPositionRate);
}


std::vector<std::string> NVRemoteConnection::getListForCommand(const std::string& commandName)
{
                commandStream.str("");
                commandStream << "setVerbosity 0" << std::endl
                                << commandName << std::endl;
                m_pSocket->send(commandStream.str());
                std::string jointNames = "";
                std::vector<std::string> jointList(0, "");

                if (this->receiveAnswer(jointNames) && (jointNames.length() > 0))
                {
                                // split the answer string into separate jointName strings
                                // each name is separated from the other by a newline char.
                                std::string::size_type firstIndex = jointNames.find_first_not_of("\n");
                                std::string::size_type nextIndex = 0;
                                while (std::string::npos != firstIndex)
                                {
                                                nextIndex = jointNames.find_first_of("\n", firstIndex);
                                                if (std::string::npos == nextIndex)
                                                                nextIndex = jointNames.length();
                                                jointList.push_back(jointNames.substr(firstIndex, (nextIndex - firstIndex - 1)));
                                                firstIndex = jointNames.find_first_not_of("\n", nextIndex);
                                }
                }
                return jointList;
}


std::vector<std::string> NVRemoteConnection::getJointList()
{
                return getListForCommand("jlist");
}


std::vector<std::string> NVRemoteConnection::getBodyList()
{
                return getListForCommand("blist");
}


void NVRemoteConnection::set3DVectorForCommandAndNode(const std::vector<float>& values, const std::string& commandName, const std::string& nodeName)
{
                assert(3 == values.size() && "NVRemoteConnection::setVector3DForCommandAndNode() -> value vector does not have 3 elements!");

                commandStream.str("");
                commandStream << commandName << " " << nodeName
                << " " << values.at(0)
                << " " << values.at(1)
                << " " << values.at(2)
                << std::endl;
                m_pSocket->send(commandStream.str());
}


void NVRemoteConnection::setValueForCommandAndNode(const float& value, const std::string& commandName, const std::string& nodeName)
{
                commandStream.str("");
                commandStream << commandName << " " << nodeName
                << " " << value
                << std::endl;
                m_pSocket->send(commandStream.str());
}


void NVRemoteConnection::setJointForce(const std::string& jointName, float force)
{
                setValueForCommandAndNode(force, "jointforce", jointName);
}


void NVRemoteConnection::setJointForce2(const std::string& jointName, float force2)
{
                setValueForCommandAndNode(force2, "jointforce2", jointName);
}


void NVRemoteConnection::setJointSpeed(const std::string& jointName, float speed)
{
                setValueForCommandAndNode(speed, "jointspeed", jointName);
}


void NVRemoteConnection::setJointSpeed2(const std::string& jointName, float speed2)
{
                setValueForCommandAndNode(speed2, "jointspeed2", jointName);
}


void NVRemoteConnection::setJointTorque(const std::string& jointName, float torque)
{
                setValueForCommandAndNode(torque, "jointtorque", jointName);
}


void NVRemoteConnection::setJointTorque2(const std::string& jointName, float torque2)
{
                setValueForCommandAndNode(torque2, "jointtorque2", jointName);
}


bool NVRemoteConnection::receiveAnswer(std::string& answer)
{
                answer.clear();

                const int LENGTH = 4096;
                int rLength = 0;
                char res[LENGTH];
                memset(res, 0, LENGTH);

                if( !this->m_pSocket->setNonBlocking(-1))
                                return false;
                while (0 < (rLength = m_pSocket->receive(res, LENGTH)))
                {
                                if (NULL != res)
                                                answer += std::string(res, rLength);
                                else
                                {
                                                answer.clear();
                                                std::cout << "NVRemoteConnection::receiveAnswer(): Buffer was NULL" << std::endl;
                                                return false;
                                }
                                if (!this->m_pSocket->setNonBlocking(0))
                                                return false;
                }

                return true;
}


bool NVRemoteConnection::connect()
{
                return m_pSocket->openSocket();
}


bool NVRemoteConnection::isConnected()
{
                return m_pSocket->isConnected();
}

---