commit 4f33861787a785c93ab2c28bc533e43ec4d3cc41 Author: philipp schoenberger Date: Tue Jun 9 11:13:59 2015 +0200 init new version without opencl diff --git a/lwrserv/Makefile b/lwrserv/Makefile new file mode 100755 index 0000000..fbf6bf3 --- /dev/null +++ b/lwrserv/Makefile @@ -0,0 +1,82 @@ +################################################################################ +# Contents: makefile with dependency generation # +# 26.05.2015 # +#------------------------------------------------------------------------------# +# makefile for cpp/c bins and libs # +# autor Philipp Schoenberger # +# ph.schoenberger@googlemail.com # +################################################################################ +LIBS = pthread + +OUT_BINARY = lwrserv + +CC = g++ +GLOBAL_FLAGS = -g -o2 -ansi -pedantic -DHAVE_SOCKLEN_T +CFLAGS = $(GLOBAL_FLAGS) +CXXFLAGS = $(GLOBAL_FLAGS) +CPPFLAGS = $(GLOBAL_FLAGS) +LDFLAGS = $(patsubst %,-l%,$(LIBS)) + +BUILD_DIR = build + +# Sources +SRC_C := $(wildcard *.c) +SRC_H := $(wildcard *.h) +SRC_CPP := $(wildcard *.cpp) +SRC_HPP := $(wildcard *.hpp) +#SRC_CXX := $(wildcard *.cxx) +#SRC_CC := $(wildcard *.C) + +# Objects +OBJS_C = $(patsubst %.c,$(BUILD_DIR)/%.o,$(SRC_C)) +OBJS_CPP = $(patsubst %.cpp,$(BUILD_DIR)/%.o,$(SRC_CPP)) + + +OBJS=$(patsubst %.o,%.o,$(OBJS_C) $(OBJS_CPP)) + +# Dependencies +DEPS_H = $(patsubst %.h,$(BUILD_DIR)/%.d,$(SRC_H)) +DEPS_HPP = $(patsubst %.hpp,$(BUILD_DIR)/%.d,$(SRC_HPP)) + +DEPS=$(OBJS:%.o=%.d) +#DEPS+=$(DEPS_H) +#DEPS+=$(DEPS_HPP) + +.DEFAULT: all +Q=@ + +all: $(BUILD_DIR) $(BUILD_DIR)/$(OUT_BINARY) + +$(BUILD_DIR): + $(Q)echo " [mk] $(BUILD_DIR)" + $(Q)mkdir -p $(BUILD_DIR) + +clean: + $(Q)echo " [rm] $(BUILD_DIR)" + $(Q)rm -rf $(BUILD_DIR) + +$(BUILD_DIR)/$(OUT_BINARY): $(BUILD_DIR) $(DEPS) $(DEP_LIBS) $(OBJS) + $(Q)echo " [ld] $@" + $(Q)( $(CC) $(CFLAGS) $(OBJS) $(LDFLAGS) -o $(OUT_BINARY)) || exit 1 + +$(BUILD_DIR)/%.o : %.c $(BUILD_DIR) + $(Q)echo " [CC] $@" + $(Q)( $(CC) -c $(CFLAGS) $(CPPFLAGS) $< -o $@ ) || exit 1 + +$(BUILD_DIR)/%.o : %.cpp $(BUILD_DIR) + $(Q)echo " [CXX] $@" + $(Q)( $(CXX) -c $(CXXFLAGS) $(CPPFLAGS) $< -o $@ ) || exit 1 + +$(BUILD_DIR)/%.d: $(BUILD_DIR) %.c + $(Q)echo " [DEP] $@" + $(Q)touch $@ + $(Q)echo -n "$@ $(patsubst %.d,%.o,$@):" >$@ + $(Q)$(CC) -M $(CFLAGS) $(filter %.cpp,$^ ) | sed 's:$*.o\:::' >> $@ + +$(BUILD_DIR)/%.d: $(BUILD_DIR) %.cpp + $(Q)echo " [DEP] $@" + $(Q)touch $@ + $(Q)echo -n "$@ $(patsubst %.d,%.o,$@):" >$@ + $(Q)$(CC) -M $(filter-out -g -o0 -o1 -o2 ,$(CPPFLAGS)) $(filter %.cpp,$^) | sed 's:$*.o\:::' >> $@ + +-include $(DEPS) diff --git a/lwrserv/Point.h b/lwrserv/Point.h new file mode 100755 index 0000000..51af7f0 --- /dev/null +++ b/lwrserv/Point.h @@ -0,0 +1,14 @@ +#ifndef POINT_H +#define POINT_H + +class Point { +public: + + Point (int x=0, int y=0) { + this->x = x; + this->y = y; + } + + int x, y; +}; +#endif // POINT_H diff --git a/lwrserv/SocketObject.cpp b/lwrserv/SocketObject.cpp new file mode 100755 index 0000000..3415a04 --- /dev/null +++ b/lwrserv/SocketObject.cpp @@ -0,0 +1,417 @@ +#include "SocketObject.h" + +#include +#include +#include +#include +#include +#include + +using namespace std; + +#if defined(__msdos__) || defined(WIN32) +SocketObject::SocketObject(void) +{ +#ifdef __SOCKET_OBJECT_DEBUG + cout << "Windows SocketObject Konstruktor" << endl; +#endif + WSADATA wsaData; + WORD wVersionRequested; + + wVersionRequested = MAKEWORD( 2, 0 ); + _skSocket = INVALID_SOCKET; + WSAStartup(wVersionRequested, &wsaData); +} + +SocketObject::SocketObject(SocketObject& sk) +{ + WSADATA wsaData; + WORD wVersionRequested; + + wVersionRequested = MAKEWORD( 2, 0 ); + _skSocket = sk._skSocket; + sk._skSocket = INVALID_SOCKET; + WSAStartup(wVersionRequested, &wsaData); +} + +SocketObject::~SocketObject(void) +{ + Disconnect(); +} +#endif + + +void SocketObject::setBlockingMode(bool nonblock) +{ + int errorCode = 0; +#if defined(__msdos__) || defined(WIN32) + u_long iMode = 1; + + if (!nonblock) + iMode=0; + + if (0 != ioctlsocket(_skSocket, FIONBIO, &iMode)) + { + errorCode = WSAGetLastError(); + cout << "Set blocking mode ("< bind(_skSocket, (sockaddr*) &saServerAddress, sizeof(sockaddr))) + { + // Socket will nicht mehr lesen und schreiben + shutdown(_skSocket,2); +#ifdef __SOCKET_OBJECT_DEBUG + std::cout << "Fehler bei Bind!" << std::endl; +#endif + return -1; + } else + { + return 0; + } +#endif +} + + +int SocketObject::Listen( void ) +{ + // lauscht bei _skSocket, max Warteschlange = 2 + return listen(_skSocket, 2); +// WIN32 : ,32 +} + + +bool SocketObject::Accept(SocketObject &skAcceptSocket) +{ + struct sockaddr_in saClientAddress; + int iClientSize = sizeof(sockaddr_in); + +#if defined(__msdos__) || defined(WIN32) + + skAcceptSocket._skSocket = accept(_skSocket, (struct sockaddr*)&saClientAddress, &iClientSize); + return !( skAcceptSocket._skSocket == INVALID_SOCKET); + +#else + + skAcceptSocket._skSocket = accept( _skSocket, (sockaddr*) &saClientAddress,(socklen_t*) &iClientSize); + return !( skAcceptSocket._skSocket == -1); + +#endif +} + + +bool SocketObject::Connect(const char* szServerAddress, unsigned short int iPort, bool nonblock) +{ +#if defined(__msdos__) || defined(WIN32) + + struct sockaddr_in server_addr; + LPHOSTENT lpHost; + int error; + + // Open the socket + _skSocket = socket(AF_INET, SOCK_STREAM, 0); + if (_skSocket == INVALID_SOCKET) + return false; + + u_long iMode = 0; + if (nonblock) + iMode = 1; + ioctlsocket(_skSocket, FIONBIO, &iMode); + + memset(&server_addr,0,sizeof(struct sockaddr_in)); + server_addr.sin_family = AF_INET; + server_addr.sin_addr.s_addr = inet_addr(szServerAddress); + + if (server_addr.sin_addr.s_addr == INADDR_NONE) + { + lpHost = gethostbyname(szServerAddress); + if (lpHost != NULL) + { + server_addr.sin_addr.s_addr = ((LPIN_ADDR)lpHost->h_addr)->s_addr; + } + else + return false; + } + + server_addr.sin_port = htons(iPort); + error = connect(_skSocket, (struct sockaddr*)&server_addr, sizeof(sockaddr)); + if (error == SOCKET_ERROR) + { + Disconnect(); + return false; + } + return true; + +#else + + struct sockaddr_in server_addr; + int status; + + // Socket erschaffen + _skSocket = socket(AF_INET, SOCK_STREAM, 0); + if (_skSocket < 0) + return false; + + if (nonblock) + fcntl(_skSocket, F_SETFL, O_NONBLOCK); + + // Initilisieren der sockaddr_in- Struktur + memset(&server_addr, 0, sizeof(struct sockaddr_in)); + server_addr.sin_family = AF_INET; + // server-Adresse bergeben + server_addr.sin_addr.s_addr = inet_addr(szServerAddress); + // std::cout << inet_ntoa (server_addr.sin_addr)<< std::endl; + // Port + server_addr.sin_port = htons(iPort); + + // Verbindung herstellen + status = connect(_skSocket, (struct sockaddr*)&server_addr, sizeof(sockaddr)); + + if (status < 0) + { + shutdown(_skSocket,2); + return false; + } + return true; + +#endif +} + + +bool SocketObject::Connect(const string szServerAddress, unsigned short int iPort, bool nonblock) +{ + return Connect((char*) szServerAddress.c_str(), iPort, nonblock); +} + + +void SocketObject::Disconnect() +{ +#if defined(__msdos__) || defined(WIN32) + if (_skSocket != INVALID_SOCKET) + { + closesocket(_skSocket); + _skSocket = INVALID_SOCKET; + } +#else + close(_skSocket); +#endif +} + +int SocketObject::Recv(char *szBuffer, int iBufferLength, int iFlags) +{ + return recv(_skSocket, szBuffer, iBufferLength, iFlags); +} + +int SocketObject::Recv (string& s) +{ + char buf [MAXRECV+1]; + // cout << WSAGetLastError() << " in SocketObject::Recv, vor recv" << endl; + s = ""; + memset(buf, 0, MAXRECV+1); + int status = Recv(buf, MAXRECV, 0); + + switch (status) + { + case -1: +#ifdef __SOCKET_OBJECT_DEBUG + cout << "status: -1 errno: " << errno << " in SocketObject::Recv" << endl; +#endif + break; + case 0: + break; + + default: + s = buf; + // strip the (expected) CRLF + if(s.length() == 1) + { + if(s[s.length()-1]=='\n' || s[s.length()-1]=='\r') + s.resize(s.length()-1); + } else if(s.length() >= 2) + { + if(s[s.length()-1]=='\n' && s[s.length()-2]=='\r') + s.resize(s.length()-2); + else if(s[s.length()-1]=='\n' || s[s.length()-1]=='\r') + s.resize(s.length()-1); + } + break; + } + return status; +} + + +int SocketObject::receiveLine(char* array, int arraySize) +{ + //std::string ret; + char r; + int pos = 0; + + while (pos 0) + { + array[pos]='\0'; + return pos; + } + }else + { + array[pos] = r; + pos++; + } + } + return pos; +} + +std::string SocketObject::receiveLine() +{ + std::string ret; + char r; + + while (1) + { + //This will not work under windows if the blocking mode is non-blocking + // -1 will indicate that there is no value, it is not an error !!! + //http://msdn.microsoft.com/en-us/library/ms740121(VS.85).aspx + //For linux see: + //http://linux.about.com/library/cmd/blcmdl2_recv.htm + //If no messages are available at the socket, the receive calls wait for a message to arrive, + //unless the socket is nonblocking (see fcntl(2)) in which case the value -1 is returned and + // the external variable errno set to EAGAIN. + switch(recv(_skSocket, &r, 1, 0)) + { + case 0: // not connected anymore; + return SOCKETOBJECT_NOT_CONNECTED; + case -1: // error + return SOCKETOBJECT_RECV_ERROR; + } + + // check for carriage return or line feed (MAC: CR, Linux: LF, Windows: CRLF) + if ((r == char(13)) || (r == char(10))) + { + if (ret.length() > 0) + return ret; + } else + { + ret += r; + } + } +} + +int SocketObject::Send(const char *szBuffer, int iBufferLength, int iFlags) +{ + return send(_skSocket,szBuffer,iBufferLength, iFlags); +} + + +int SocketObject::Send (const string s) +{ + string str = s; + // add the (expected) CRLF + if(str == "") + { + str = "\n\r"; + } else if(str.size() == 1) + { + if(str[0] == '\r') + { + str = "\n\r"; + } else if(str[0] != '\n') + { + str += "\n\r"; + } + } else if(str.size() >= 2) + { + if(str[str.length()-1] != '\n' || str[str.length()-2]!='\r') + { + str += "\n\r"; + } + } +#if defined(__msdos__) || defined(WIN32) + return Send((char*) str.c_str(), (int) str.size(), 0); +#else + return Send((char*) str.c_str(), (int) str.size(), (int) MSG_NOSIGNAL); +#endif +} + diff --git a/lwrserv/SocketObject.h b/lwrserv/SocketObject.h new file mode 100755 index 0000000..28ec108 --- /dev/null +++ b/lwrserv/SocketObject.h @@ -0,0 +1,172 @@ +/** \class SocketObject +* +* \brief Die Klasse SocketObject kapselt die Verwendung von Sockets +* +* Die Klasse SocketObject liefert eine komfortable Benutzungs-Schnittstelle +* für Sockets. +* Sie bietet Methoden zum Senden und Empfangen von verschiedenen Datentypen, +* sowie Methoden zum Verbindungsauf- und abbau. +*/ +#ifndef _SOCKETOBJECT_H +#define _SOCKETOBJECT_H + +#if (defined(__msdos__) || defined(WIN32)) +#include +#else +#include +#include +#include +#include +#include +#include +#include +#endif + +#include +#include +#include + + +#define SOCKETOBJECT_NOT_CONNECTED "SOCKETOBJECT_NOT_CONNECTED" +#define SOCKETOBJECT_RECV_ERROR "SOCKETOBJECT_RECV_ERROR" + +class SocketObject { + +public: + + static const int MAXRECV = 1024; + + +private: + +#if defined(__msdos__) || defined(WIN32) + SOCKET _skSocket; +#else + int _skSocket; +#endif + + + +public: + +#if defined(__msdos__) || defined(WIN32) + // Konstruktor der Klasse + SocketObject(); + + // Copy-Konstruktor + SocketObject(SocketObject& sk); + + // Destruktor der Klasse + ~SocketObject(); +#endif + + /****** NUR SERVER - METHODEN ******/ + + /** \brief Bindet Socket an einen Port + * + * Die bind()-Methdode gibt dem Socket einen + * lokalen Namen, damit andere Prozesse mit + * ihm kommunizieren können. + * Wird nur vom Server benutzt! + * + * @param iPort Port an den gebunden werden soll + * + * @return 0 = ok, SOCKET_ERROR = Fehler + * + */ + int Bind(unsigned short int iPort); + + /** \brief Wartet auf eigehende Verbindungsanfragen + * + * Die Listen()-Methode erwartet eingehende + * Verbindungsanfragen. + * Wird nur vom Server benutzt! + * + * @return 0 = ok, SOCKET_ERROR = Fehler + * + */ + int Listen(); + + /** \brief Acceptiert eingehende Verbindungsanfrage + * + * Die accept()-Methode akzeptiert eine eingehende + * Verbindung und "startet" die Verbindung" + * Wird nur vom Server benutzt! + * + * @param skAcceptSocket zu akzeptierender Socket + */ + bool Accept(SocketObject &skAcceptSocket); + + /****** SERVER - METHODEN ENDE ******/ + + /****** NUR CLIENT - METHODEN ******/ + /** \brief Stellt Verbindsanfage an den Server + * + * Mittels der connect()-Methode stellt der Client + * eine Verbindungsanfrage an den Server her, der + * + * + * Wird nur vom Client benutzt! + * + * @param szServerAdrress Adresse des Servers + * @param iPort Port des Server + */ + bool Connect(const char *szServerAddress, unsigned short int iPort, bool nonblock); + + /** \brief Stellt Verbindsanfage an den Server + * + * Mittels der connect()-Methode stellt der Client + * eine Verbindungsanfrage an den Server her, der + * Die IP wird hier als String übergeben + * Wird nur vom Client benutzt! + * + * @param ServerAdrress Adresse des Servers + * @param iPort Port des Server + */ + bool Connect(const std::string ServerAddress, unsigned short int iPort, bool nonblock = false); + + void setBlockingMode(bool nonblock); + + std::string receiveLine(); + int receiveLine(char* array, int arraySize); + + /****** CLIENT - METHODEN ENDE ******/ + /** + * Die send()-Methode sendet Daten an den verbundenen + * Socket + * + * @param szBuffer zu sendende Nachricht + * @param iBufferLength Läge der Nachricht + * @param iFlags Flags + * + * @return Anzahl der gesendeten Bytes + */ + int Send(const char *szBuffer, int iBufferLength, int iFlags); + + /** \brief Senden eines Strings + * + * Die Methode sendet einen String an den Kommunikationspartner + * + * @param toSend String, der gesendet werden soll + * + * @return Anzahl der gesendeten Bytes + */ + int Send(const std::string toSend); + + /** \brief Empfängt Characters + * + * @param szBuffer hier wird die Nachricht reingeschrieben + * @param iBufferLength max. Läge der Nachricht + * @param iFlags Flags + * + * @return Anzahl der gesendeten Bytes + */ + int Recv(char *szBuffer, int iBufferLength, int iFlags); + int Recv (std::string& s); + + void Disconnect(); + + +}; + +#endif diff --git a/lwrserv/StringTool.cpp b/lwrserv/StringTool.cpp new file mode 100755 index 0000000..4c32d4e --- /dev/null +++ b/lwrserv/StringTool.cpp @@ -0,0 +1,36 @@ +#include "header.h" + +void StringTool::String2CmdArg(string s, string & cmd, string & arg) +{ + string message = stripWhiteSpace(s); + if (message.find_first_of(STRING_WHITESPACES, 1) != string::npos) + { + cmd = stripWhiteSpace(message.substr(0, message.find_first_of(STRING_WHITESPACES, 1))); + arg = stripWhiteSpace(message.substr(message.find_first_of(STRING_WHITESPACES, 1))); + } else + { + cmd = message; + arg = ""; + } + return; +} + +string StringTool::stripWhiteSpace(string s) +{ + unsigned int i, j; + if (s.length() > 0) + { + i = s.find_first_not_of(STRING_WHITESPACES); + j = s.find_last_not_of(STRING_WHITESPACES); + + if (i == string::npos) + i = 0; + if (j == string::npos) + j = s.length()-1; + + return string(s, i, j-i+1); + } + return string(""); +} + + diff --git a/lwrserv/StringTool.h b/lwrserv/StringTool.h new file mode 100755 index 0000000..39707c8 --- /dev/null +++ b/lwrserv/StringTool.h @@ -0,0 +1,13 @@ +#include + +#ifndef STRING_TOOL +#define STRING_TOOL + +class StringTool +{ +private: +public: + void String2CmdArg(std::string s, std::string & cmd, std::string & arg); + std::string stripWhiteSpace(std::string s); +}; +#endif diff --git a/lwrserv/SvrHandling.cpp b/lwrserv/SvrHandling.cpp new file mode 100755 index 0000000..b2014e2 --- /dev/null +++ b/lwrserv/SvrHandling.cpp @@ -0,0 +1,1079 @@ +#include "header.h" +#include +#include "global.h" +#include "mat.h" + +Mat4f getProbeTrans(){ + Mat4f Transf; + Transf(0,0) = 0.72994228; + Transf(0,1) = 0.01910900; + Transf(0,2) = 0.67421692; + Transf(0,3) = 32.00235858/1000.0; + + Transf(1,0) = 0.00313707; + Transf(1,1) = 0.98508639; + Transf(1,2) = -0.02537338; + Transf(1,3) = -9.29382467/1000.0; + + Transf(2,0) = -0.67612835; + Transf(2,1) = 0.02308023; + Transf(2,2) = 0.72720430; + Transf(2,3) = 152.32390224/1000.0; + Transf(3,3) = 1.0; + + return Transf; +} + +void mattoabc(float M[3][4], float &a, float &b, float &c) +{ + float norm; + float sa; + float ca; + + norm = sqrt((M[0][0]*M[0][0])+(M[1][0]*M[1][0])); + + if (norm>1e-5) + { + sa = M[1][0]/norm; + ca = M[0][0]/norm; + a = atan2(sa,ca); + } else { + sa = 0; + ca = 1; + a = 0; + } + b = atan2(-M[2][0],ca*M[0][0]+sa*M[1][0]); + c = atan2(sa*M[0][2]-ca*M[1][2],-sa*M[0][1]+ca*M[1][1]); +} + +void debugMat(Mat4f M) +{ +#if 0 + for (int i=0;iabs(maxRangeJnt[i])) + { + // join angle is too big + return false; + } + } + // no join angle is too big + return false; +} + +Mat4f vecToMat(float vec[12]) +{ + Mat4f result; + for (int i=0; i<3; i++) + { + for (int j=0; j<4; j++) + { + result(i,j) = vec[i*4+j]; + } + } + result(3,3)= 1.0f; + return result; +} + +float* matToVec(Mat4f mat) +{ + float* vec = new float[12]; + for (int j=0;j<3;j++) + { + for (int k=0;k<4;k++) + { + vec[j*4+k] = mat(j,k); + } + } + return vec; +} + +Mat4f getTranslation(double tx, double ty, double tz) +{ + Mat4f transl; + transl(0,0) = 1; + transl(1,1) = 1; + transl(2,2) = 1; + + transl(0,3) = tx; + transl(1,3) = ty; + transl(2,3) = tz; + transl(3,3) = 1; + return transl; +} + +SvrHandling::SvrHandling() +{ +} + +SvrHandling::~SvrHandling() +{ +} + +void SvrHandling::run(int port) +{ + cout << timestamp() + "Starting " << SVR_NAME << "\n"; + port = SVR_DEFAULT_PORT; + while (c_state !=done) + { + SocketObject *socket = new SocketObject; + if (socket->Bind(port) <0) + { + exit (1); + } + socket->Listen(); + while(1) + { + SocketObject *client = new SocketObject; + if (socket->Accept(*client)) + { + cout << timestamp() + "Client accepted!\n"; + if (handshakeAccepted(*client)) + { + handle(*client); + } + client->Disconnect(); + cout << timestamp() + "Client disconnected.\n"; + } + else + { + cout << timestamp() + "Client bin error.\n"; + } + delete client; + } + delete socket; + } +} + +bool SvrHandling::handshakeAccepted(SocketObject& client) +{ + this->c_state = handshake; + string message = SVR_HELLO_MSG; + client.Send(message); + client.Recv(message); + if (message.find(SVR_HANDSHAKE,0) != string::npos) + { + c_state = accepted; + message = SVR_ACCEPTED; + } + else + { + c_state = waiting; + message = SVR_FAILED; + cout << timestamp() + "Handshake failed.\n"; + } + client.Send(message); + return(c_state == accepted); +} + +void SvrHandling::handle(SocketObject& client) +{ + string message, cmd, arg; + while (c_state == accepted) + { + if (client.Recv(message) > 0) + { + cout< tokens; + int i = 0; + while (ss >> buf) + { + if (i>LBR_MNJ-1) + { + client.Send(SVR_FALSE_RSP); + return; + } + tokens.push_back(buf); + ss2f.flush(); + ss2f.clear(); + ss2f << buf; + ss2f >> temp[i]; + i++; + } + if (i tokens; + int i = 0; + while (ss >> buf) + { + if (i>15-1) + { + client.Send(SVR_FALSE_RSP); + return; + } + tokens.push_back(buf); + ss2f.flush(); + ss2f.clear(); + ss2f << buf; + ss2f >> temp[i]; + i++; + } + if (i<15-1) + { + client.Send(SVR_FALSE_RSP); + return ; + } + for (int i=0 ;i<3; i++) + { + for (int j=0;j<4;j++) + { + MSRCMDPOSE[i][j]=temp[i*4+j]; + } + } + float arg[3]; + for (int i=0;i<3;i++) + { + arg[i] = temp[12+i]; + } + + //Backward Calculation + + double* CalcJoints=NULL; + CalcJoints = kukaBackwardCalc(temp, arg); + + //Check for Joint Range + if (!checkJntRange(CalcJoints)) + { + string out = "Error: Joints out of Range!"; + client.Send(out); + return; + } + + // Jmove + for (int i=0 ;i tokens; + int i = 0; + if (args == "") + { + client.Send(SVR_FALSE_RSP); + return; + } + while (ss >> buf) + { + if (i>0) + { + client.Send(SVR_FALSE_RSP); + return; + } + tokens.push_back(buf); + ss2f.flush(); + ss2f.clear(); + ss2f << buf; + ss2f >> temp[i]; + i++; + } + if ((temp[0] < 1)||(temp[0]>1000)) + { + client.Send(SVR_FALSE_RSP); + return; + } + VELOCITY = temp[0]; + client.Send(SVR_TRUE_RSP); + __SVR_CURRENT_JOB = false; +} + +void SvrHandling::SetAccel(SocketObject& client, string& args) +{ + __SETACCEL = true; + __SVR_CURRENT_JOB = true; + string buf; + float temp[1]; + stringstream ss(args); + stringstream ss2f; + vector tokens; + int i = 0; + + if (args == "") + { + client.Send(SVR_FALSE_RSP); + return; + } + while (ss >> buf) + { + if (i>0) + { + client.Send(SVR_FALSE_RSP); + return; + } + tokens.push_back(buf); + ss2f.flush(); + ss2f.clear(); + ss2f << buf; + ss2f >> temp[i]; + i++; + } + if ((temp[0] < 1)||(temp[0]>100)) + { + client.Send(SVR_FALSE_RSP); + return; + } + ACCELARATION = temp[0]; + client.Send(SVR_TRUE_RSP); + __SVR_CURRENT_JOB = false; +} + +void SvrHandling::StartPotFieldMode(SocketObject& client, string& args) +{ + __MSRSTARTPOTFIELD = false; + __MSRSTOPPOTFIELD = false; + //Set current Joint Vals + for (int i=0; i tokens; + int i = 0; + while (ss >> buf) + { + if (i>15-1) + { + client.Send(SVR_FALSE_RSP); + return; + } + tokens.push_back(buf); + ss2f.flush(); + ss2f.clear(); + ss2f << buf; + ss2f >> temp[i]; + i++; + } + if (i<15-1) + { + client.Send(SVR_FALSE_RSP); + return ; + } + for (int i=0 ;i<3; i++) + { + for (int j=0;j<4;j++) + { + MSRCMDPOSE[i][j]=temp[i*4+j]; + } + } + float arg[3]; + for (int i=0;i<3;i++) + { + arg[i] = temp[12+i]; + } + + //Backward Calculation + + double* CalcJoints=NULL; + CalcJoints = kukaBackwardCalc(temp, arg); + + //Check for Joint Range + if (!checkJntRange(CalcJoints)) + { + string out = "Error: Joints out of Range!"; + client.Send(out); + return; + } + + // Set global Position + __MSRSETPOS = true; + globi = 1; + for (int i=0 ;i tokens; + int i = 0; + + while (ss >> buf) + { + if (i>LBR_MNJ-1) + { + client.Send(SVR_FALSE_RSP); + return; + } + tokens.push_back(buf); + ss2f.flush(); + ss2f.clear(); + ss2f << buf; + ss2f >> temp[i]; + i++; + } + if (i tokens; + int i = 0; + + while (ss >> buf) + { + if (i>15-1) + { + client.Send(SVR_FALSE_RSP); + return; + } + tokens.push_back(buf); + ss2f.flush(); + ss2f.clear(); + ss2f << buf; + ss2f >> temp[i]; + i++; + } + if (i<15-1) + { + client.Send(SVR_FALSE_RSP); + return ; + } + + float arg[3]; + for (int i=0;i<3;i++) + { + arg[i] = temp[12+i]; + } + + //Calculating end-effector position for target "TRobot" + Mat4f Tsurface; + Tsurface = vecToMat(temp); + Mat4f TransfUS = getProbeTrans(); + Mat4f InvTransf; + InvTransf = TransfUS.inv(); + Mat4f TRobot; + TRobot = Tsurface*InvTransf; + + //Calculating end-effector position for approach position "ApRobot" + Mat4f ApRobot; + Mat4f ApSurface; + Mat4f TransZ; + // 100mm approach position + TransZ = getTranslation(0,0,-0.1); + ApSurface = Tsurface * TransZ; + ApRobot = ApSurface * InvTransf; + float* tempVec = NULL; + tempVec = matToVec(TRobot); + for (int i=0; i<12; i++) + USTarget[i] = tempVec[i]; + //tempVec = matToVec(ApRobot); + for (int i=0; i<12; i++) + USApproach[i] = tempVec[i]; + + client.Send(SVR_TRUE_RSP); + __DOUS2 = true; + + + + +} + +void SvrHandling::DoUltrasound(SocketObject& client, string& args) +{ + __SVR_CURRENT_JOB = true; + string buf; + float temp[15]; + stringstream ss(args); + stringstream ss2f; + vector tokens; + int i = 0; + + while (ss >> buf) + { + if (i>15-1) + { + client.Send(SVR_FALSE_RSP); + return; + } + tokens.push_back(buf); + ss2f.flush(); + ss2f.clear(); + ss2f << buf; + ss2f >> temp[i]; + i++; + } + if (i<15-1) + { + client.Send(SVR_FALSE_RSP); + return ; + } + + float arg[3]; + for (int i=0;i<3;i++) + { + arg[i] = temp[12+i]; + } + + //Calculating end-effector position for target "TRobot" + Mat4f Tsurface; + Tsurface = vecToMat(temp); + Mat4f TransfUS = getProbeTrans(); + Mat4f InvTransf; + InvTransf = TransfUS.inv(); + Mat4f TRobot; + //10cm tiefer als Sollposition + Mat4f TransZ; + TransZ = getTranslation(0.0f ,0.0f,0.1f); + TRobot = Tsurface*TransZ*InvTransf; + //TRobot = Tsurface*InvTransf; + //Calculating end-effector position for approach position "ApRobot" + Mat4f ApRobot; + Mat4f ApSurface; + /*Mat TransZ;*/ + + // 100mm approach position + TransZ = getTranslation(0,0,-0.1); + ApSurface = Tsurface * TransZ; + ApRobot = ApSurface * InvTransf; + float* tempVec = NULL; + tempVec = matToVec(TRobot); + + for (int i=0; i<12; i++) + USTarget[i] = tempVec[i]; + tempVec = matToVec(ApRobot); + for (int i=0; i<12; i++) + USApproach[i] = tempVec[i]; + + client.Send(SVR_TRUE_RSP); + __DOUS2 = true; + +} + +void SvrHandling::quit(SocketObject& client) +{ + client.Send(SVR_TRUE_RSP); + client.Disconnect(); +} + +void SvrHandling::debug(SocketObject& client) +{ + + __DEBUG = true; + + //check = true; + //float temp[7]; + //while (1){ + // friInst.setToKRLInt(15,1); + // friInst.doSendData(); + // //client.Send(friInst.getFrmKRLInt(2)); + // for ( int i= 0; i < LBR_MNJ; i++) + //{ + // temp[i]=friInst.getMsrMsrJntPosition()[i]; + //} + //friInst.doPositionControl(temp); + //} +} diff --git a/lwrserv/SvrHandling.h b/lwrserv/SvrHandling.h new file mode 100755 index 0000000..fd852be --- /dev/null +++ b/lwrserv/SvrHandling.h @@ -0,0 +1,61 @@ +#include "defines.h" +#include "StringTool.h" +#include +#include "friremote.h" + +#ifndef SVR_HANDLING +#define SVR_HANDLING + +// Serve status +typedef enum {waiting, handshake, accepted, done} svr_state; + +class SvrHandling : private StringTool +{ +private: + svr_state c_state; + //Handshake + bool handshakeAccepted(SocketObject& client); + //Handle client + void handle(SocketObject& client); + //Handling request for current Joint Values + void GetPositionJoints(SocketObject& client); + //Get Position as POSE Matrix + void GetPositionHomRowWise(SocketObject& client); + //Get Force/torque values from TCP + void GetForceTorqueTcp(SocketObject& client); + //Move to given Joint combination + void MovePTPJoints(SocketObject& client, std::string& args); + //Move to given POSE position + void MoveHomRowWiseStatus(SocketObject& client, std::string& args); + //Set Velocity + void SetSpeed(SocketObject& client, std::string& args); + //Set Acceleration + void SetAccel(SocketObject& client, std::string& args); + //Starting Potential Field Movement Mode + void StartPotFieldMode(SocketObject& client, std::string& args); + //Stopping Potential Field Movement Mode + void StopPotFieldMode(SocketObject& client, std::string& args); + //Setting Target Position HomRowWise for Potential Move + void SetPos(SocketObject& client, std::string& args); + //Setting Target Position as Joints for Potential Move + void SetJoints(SocketObject& client, std::string& args); + //Cartesian Movement + //Move to given POSE position + void MoveCartesian(SocketObject& client, std::string& args); + //Do Ultrasound + void DoUltrasound(SocketObject& client, std::string& args); + //Quit + void quit(SocketObject& client); + //DEBUGGING + void debug(SocketObject& client); +public: + //Constructor + SvrHandling(); + //Destructor + virtual ~SvrHandling(); + //Rund server @port + void run(int port); + //Get current timestamp + std::string timestamp(); +}; +#endif diff --git a/lwrserv/defines.h b/lwrserv/defines.h new file mode 100755 index 0000000..1048399 --- /dev/null +++ b/lwrserv/defines.h @@ -0,0 +1,38 @@ + +#ifndef __DEFINES_H__ +#define __DEFINES_H__ + +#define SVR_NAME "lwrsvr 4.11d" +#define SVR_DEFAULT_PORT 8000 +#define SVR_HANDSHAKE "Hello Robot" +#define SVR_HELLO_MSG std::string("welcome to ") + SVR_NAME + ("!") +#define SVR_ACCEPTED std::string("accepted") +#define SVR_FAILED std::string("Failed.") +#define STRING_WHITESPACES " \t\r\n" +#define SVR_TRUE_RSP "true" +#define SVR_FALSE_RSP "false" +#define M_PI 3.141592654f + + + + +//Begin SVR_COMMANDS +#define CMD_GetPositionJoints "GPJ" +#define CMD_GetPositionHomRowWise "GPHRW" +#define CMD_GetForceTorqueTcp "GFT" +#define CMD_MovePTPJoints "MPTPJ" +#define CMD_MoveHomRowWiseStatus "MHRWS" +#define CMD_SetSpeed "SS" +#define CMD_SetAccel "SA" +#define CMD_StartPotFieldMode "STPF" +#define CMD_StopPotFieldMode "SPPF" +#define CMD_SetPos "SP" +#define CMD_SetJoints "SJ" +#define CMD_QUIT "Quit" +#define CMD_ISKUKA "IsKukaLWR" +#define CMD_MoveCartesian "MC" +#define CMD_DoUltrasound "DU" +//End SVR_COMMANDS + +#endif /* __DEFINES_H__ */ + diff --git a/lwrserv/friComm.h b/lwrserv/friComm.h new file mode 100755 index 0000000..fab420b --- /dev/null +++ b/lwrserv/friComm.h @@ -0,0 +1,504 @@ +/*{{[PH] +**************************************************************************** + Project: FRI + + This material is the exclusive property of KUKA Roboter GmbH + and must be returned to KUKA Roboter GmbH immediately upon + request. This material and the information illustrated or + contained herein may not be used, reproduced, stored in a + retrieval system, or transmitted in whole or in part in any + way - electronic, mechanical, photocopying, recording, or + otherwise, without the prior written consent of KUKA Roboter GmbH. + + All Rights Reserved + Copyright (C) 2009 + KUKA Roboter GmbH + Augsburg, Germany + +[PH]}} +*/ + +/* +{{[FH] +**************************************************************************** + friComm.h + + NOTE: FRI (Fast Research inteface) is subject to radical change + + +[FH]}} +*/ +/** + \author (Andreas Stemmer,DLR) + \author (Guenter Schreiber,KUKA) + \file + \brief This header file describes the basic communcation structures for + the FastRobotInterface + + + - all data is transmitted in Intel byte order (little endian) + - structs use 4 byte padding + - all floats are IEEE 754 single precision floats with 4 bytes + - for the sequence counters in the header, the number following + #FRI_UINT16_MAX is assumed to be zero (normal integer overflow) + - For Checking, that your compiler setup interprets the binary communication as expceted, + use the macros #FRI_CHECK_SIZES_OK, #FRI_PREPARE_CHECK_BYTE_ORDER, #FRI_CHECK_BYTE_ORDER_OK in your code. + + */ +/***************************************************************************** + + friComm.h + + created: 09/05/26 by Andreas Stemmer + + $Revision: 1.17 $ $Author: stemmer $ $Date: 2010/02/12 15:15:18 $ + + (C) by Institute of Robotics and Mechatronics, DLR + + Description: + + This header file describes the basic communcation structures for + the FastRobotInterface + + Note: + + + + *****************************************************************************/ + +#ifndef FRICOMM_H +#define FRICOMM_H + + +#define LBR_MNJ 7 /*!< Number of Managed Joints for Light Weight Robot */ +#define FRI_USER_SIZE 16 /*!< Number of Userdefined data, which are exchanged to KRL Interpreter */ +#define FRI_CART_FRM_DIM 12 /*!< Number of data within a 3x4 Cartesian frame - homogenous matrix */ +#define FRI_CART_VEC 6 /*!< Number of data for a Cartesian vector */ + + +typedef unsigned char fri_uint8_t; /*!< 1 byte unsigned */ +typedef unsigned short fri_uint16_t; /*!< 2 byte unsigned */ +typedef unsigned int fri_uint32_t; /*!< 4 byte unsigned */ +typedef int fri_int32_t; /*!< 4 byte signed */ +typedef float fri_float_t; /*!< 4 byte IEEE 754 float */ +#define FRI_UINT16_MAX ((1<<16) - 1) /*!< Biggest UINT16 w.r.t the interface - porting isno fun */ + + +/** header data used in every packet in both directions + * + * */ +typedef struct +{ + /** sequence counter (increasing with every packet) of the sender + Used for detecting packet loss of UDP Transfer, + put in your code s.th. like + * fri_uint16_t seqCount; + * seqCount++; + * cmd.head.sendSeqCount=seqCount; + */ + fri_uint16_t sendSeqCount; + + /** reflected sequence counter (mirrored from last received packet) + Used for detecting packet loss of UDP transfer and latency statistics + \sa tFriIntfStatistics + * remote side: put s.th. like + * cmd.head.reflSeqCount=msr.head.sendSeqCount; + */ + fri_uint16_t reflSeqCount; + + /** overall packet size + Remote side: FRI_CMD_DATA_SIZE + */ + fri_uint16_t packetSize; + + /** unique datagram id to detect version problems etc. + Remote side: FRI_DATAGRAM_ID_CMD + */ + fri_uint16_t datagramId; +} tFriHeader; +#define FRI_HEADER_SIZE 8 + + + + +/** data for direct interaction with KRL */ +typedef struct +{ + /** 16 float values (corresponds to $FRI_TO_REA[] and $FRI_FRM_REA[]) */ + fri_float_t realData[FRI_USER_SIZE]; + + /** 16 int values (corresponds to $FRI_TO_INT[] and $FRI_FRM_INT[]) */ + fri_int32_t intData[FRI_USER_SIZE]; + + /** 16 bool values, stored in one int */ + fri_uint16_t boolData; + + /** manual padding to a multiple of 4 bytes */ + fri_uint16_t fill; +} tFriKrlData; +#define FRI_KRL_DATA_SIZE 132 + + + + +/** reported communication statistics (average rating over the last + 100 packets) */ +typedef struct +{ + /** avg. answer rate (should be close to 1) */ + fri_float_t answerRate; + + /** avg. latency in seconds (should be as small as possible) */ + fri_float_t latency; + + /** avg. jitter in seconds (should be as small as possible) */ + fri_float_t jitter; + + /** avg. missing answer packets (should be as small as possible) */ + fri_float_t missRate; + + /** absolute missing answer packets */ + fri_uint32_t missCounter; +} tFriIntfStatistics; +#define FRI_INTF_STATISTICS_SIZE 20 + + + + +/** feedback about the interface itself */ +typedef struct +{ + /** current system time in seconds */ + fri_float_t timestamp; + + /** state of interface (monitor or command FRI_STATE) */ + fri_uint16_t state; + + /** quality of communication (FRI_QUALITY) */ + fri_uint16_t quality; + + /** configured sample time for sent measurment packets */ + fri_float_t desiredMsrSampleTime; + + /** configured sample time for received command packets */ + fri_float_t desiredCmdSampleTime; + + /** configured safety limits */ + fri_float_t safetyLimits; + + /** communication statistics */ + tFriIntfStatistics stat; +} tFriIntfState; +#define FRI_INTF_STATE_SIZE (20 + FRI_INTF_STATISTICS_SIZE) + + + + +/** these are the states that are reported */ +typedef enum +{ + FRI_STATE_INVALID =-1, /**< State invalid e.g. not initialized */ + FRI_STATE_OFF = 0, /**< internal state only (no active interface) */ + FRI_STATE_MON = 1, /**< FRI is in monitor mode */ + FRI_STATE_CMD = 2 /**< FRI is in command mode */ +} FRI_STATE; + + + + +/** quality of the connection is classified in four steps */ +typedef enum +{ + FRI_QUALITY_INVALID =-1, /**< not allowed, since improber initialized */ + FRI_QUALITY_UNACCEPTABLE = 0, /**< commanding is not allowed */ + FRI_QUALITY_BAD = 1, /**< commanding is allowed */ + FRI_QUALITY_OK = 2, /**< commanding is allowed */ + FRI_QUALITY_PERFECT = 3 /**< commanding is allowed */ +} FRI_QUALITY; + + + + +/** currently active controller */ +typedef enum +{ + /** only joint position can be commanded */ + FRI_CTRL_POSITION = 1, + /** joint/cart positions, joint/cart stiffness, joint/cart damping + and additional TCP F/T can be commanded */ + FRI_CTRL_CART_IMP = 2, + /** joint positions, stiffness and damping and additional joint + torques can be commanded */ + FRI_CTRL_JNT_IMP = 3, + /** for all the other modes... */ + FRI_CTRL_OTHER = 0 +} FRI_CTRL; + + + + +/** feedback about the robot */ +typedef struct +{ + /** power state of drives (bitfield) */ + fri_uint16_t power; + + /** selected control strategy (FRI_CTRL) */ + fri_uint16_t control; + + /** drive errors (leads to power off) */ + fri_uint16_t error; + + /** drive warnings */ + fri_uint16_t warning; + + /** temperature of drives */ + fri_float_t temperature[LBR_MNJ]; +} tFriRobotState; +#define FRI_ROBOT_STATE_SIZE 36 + + + + +/** current robot data */ +typedef struct +{ + /** measured joint angles (in rad) + * KRL: $AXIS_ACT_MSR + * */ + fri_float_t msrJntPos[LBR_MNJ]; + + /** measured Cartesian frame (in m) + * KRL: $POS_ACT_MSR + * Reference: Base and tool are specified by $stiffness.base, $stiffness.tool + * */ + fri_float_t msrCartPos[FRI_CART_FRM_DIM]; + + /** commanded joint angle (in rad, before FRI) + * KRL: $AXIS_ACT_CMD + * */ + fri_float_t cmdJntPos[LBR_MNJ]; + + /** commanded joint offset (in rad, due to FRI) */ + fri_float_t cmdJntPosFriOffset[LBR_MNJ]; + + /** commanded Cartesian frame (in m, before FRI) + * KRL: $POS_ACT_CMD + * Reference: Base and tool are specified by $stiffness.base, $stiffness.tool + * */ + fri_float_t cmdCartPos[FRI_CART_FRM_DIM]; + + /** commanded Cartesian frame (in m, due to FRI) */ + fri_float_t cmdCartPosFriOffset[FRI_CART_FRM_DIM]; + + /** measured joint torque (in Nm) + * KRL $TORQUE_AXIS_ACT + * */ + fri_float_t msrJntTrq[LBR_MNJ]; + + /** estimated external torque (in Nm) + * KRL: $TORQUE_AXIS_EST + * */ + fri_float_t estExtJntTrq[LBR_MNJ]; + + /** estimated TCP force/torque (N, Nm) + KRL: $TORQUE_TCP_EST + - reference frame: $STIFFNESS.TASKFRAME + - Layout: Fx, Fy, Fz, Tz, Ty, Tx + */ + fri_float_t estExtTcpFT[FRI_CART_VEC]; + + /** Jacobian matrix + * reference frame: $STIFFNESS.TASKFRAME + * row major (#FRI_CART_VEC x #LBR_MNJ) + * You should be able to cast it directly into a C-matrix with the layout of + J[FRI_CART_VEC][LBR_MNJ] + * Or copy it like + for ( int i = 0; i < FRI_CART_VEC; i++) + for ( int j = 0; j < LBR_MNJ; j++) + J[i][j] = jacobian[i*LBR_MNJ+j] + * Interpretation of the colums [ _X_ _Y_ _Z_ _RZ_ _RY_ _RX_] + * The Jacobian is generated by "geometric" reasoning with a corresponding physical + interpretation of instantaneous angular velocity + */ + fri_float_t jacobian[FRI_CART_VEC*LBR_MNJ]; + + /** mass matrix */ + fri_float_t massMatrix[LBR_MNJ*LBR_MNJ]; + + /** gravity (in m/s^2) */ + fri_float_t gravity[LBR_MNJ]; +} tFriRobotData; +#define FRI_ROBOT_DATA_SIZE (175*4) /**< Binary size of #tFriRobotData */ + + + +/** Identification constant for the structure #tFriMsrData + Note: This datagram id will be abused for versioning as well */ +#define FRI_DATAGRAM_ID_MSR 0x2006 +/** Data that is sent from the KRC to the external controller. + The structure is identified by #FRI_DATAGRAM_ID_MSR + */ +typedef struct +{ + /** the header */ + tFriHeader head; + + /** data from KRL */ + tFriKrlData krl; + + /** state of interface */ + tFriIntfState intf; + + /** robot state */ + tFriRobotState robot; + + /** robot data */ + tFriRobotData data; +} tFriMsrData; +#define FRI_MSR_DATA_SIZE (FRI_HEADER_SIZE + FRI_KRL_DATA_SIZE + \ + FRI_INTF_STATE_SIZE + FRI_ROBOT_STATE_SIZE + \ + FRI_ROBOT_DATA_SIZE) + + + + +/** The bitfield is subject to change, any use of hardcoded constants + is NOT RECOMMENDED! Use the provided defines instead! */ +#define FRI_CMD_JNTPOS 0x0001 +// Currently not supported #define FRI_CMD_JNTVEL 0x0002 +#define FRI_CMD_JNTTRQ 0x0004 +#define FRI_CMD_JNTSTIFF 0x0010 +#define FRI_CMD_JNTDAMP 0x0020 + +#define FRI_CMD_CARTPOS 0x0100 +//Currently not supported #define FRI_CMD_CARTVEL 0x0200 +#define FRI_CMD_TCPFT 0x0400 +#define FRI_CMD_CARTSTIFF 0x1000 +#define FRI_CMD_CARTDAMP 0x2000 + + + + +/** new robot commands */ +typedef struct +{ + /** bitfield which selects which commanded values are relevant */ + fri_uint32_t cmdFlags; + + /** commanded joint angle (in rad) + #FRI_CMD_JNTPOS + */ + fri_float_t jntPos[LBR_MNJ]; + + /** commanded Cartesian frame (in m) + #FRI_CMD_CARTPOS + */ + fri_float_t cartPos[FRI_CART_FRM_DIM]; + + /** commanded additional joint torque (in Nm) + #FRI_CMD_JNTTRQ*/ + fri_float_t addJntTrq[LBR_MNJ]; + + /** commanded additional TCP force/torque (N, Nm) + #FRI_CMD_TCPFT + * reference frame: $STIFFNESS.TASKFRAM + * Layout [Fx, Fy, Fz, Tz, Ty, Tx]*/ + fri_float_t addTcpFT[FRI_CART_VEC]; + + /** joint stiffness (Nm/rad) + #FRI_CMD_JNTSTIFF */ + fri_float_t jntStiffness[LBR_MNJ]; + + /** joint damping (normalized) + #FRI_CMD_JNTDAMP*/ + fri_float_t jntDamping[LBR_MNJ]; + + /** Cartesian stiffness (N/m, Nm/rad) + * #FRI_CMD_CARTSTIFF + * Layout [Cx, Cy, Cz, Ca(Rz), Cb(Ry), Cc(Rx)]*/ + fri_float_t cartStiffness[FRI_CART_VEC]; + + /** Cartesian damping (normalized) + * #FRI_CMD_CARTDAMP + * Layout [Dx, Dy, Dz, Da(Rz), Db(Ry), Dc(Rx)]*/ + fri_float_t cartDamping[FRI_CART_VEC]; +} tFriRobotCommand; +#define FRI_ROBOT_COMMAND_SIZE (59*4) + + + +/** Symbolic define to identify #tFriCmdData + Note: This datagram id will be abused for versioning as well */ +#define FRI_DATAGRAM_ID_CMD 0x1005 +/** The commanding structure, which is sent to the KRC Side. + Identified by #FRI_DATAGRAM_ID_CMD */ +typedef struct +{ + /** the header */ + tFriHeader head; + + /** data to KRL */ + tFriKrlData krl; + + /** robot commands */ + tFriRobotCommand cmd; +} tFriCmdData; +#define FRI_CMD_DATA_SIZE (FRI_HEADER_SIZE + FRI_KRL_DATA_SIZE + FRI_ROBOT_COMMAND_SIZE) + + + + +/** a convenience macro for the user to check if padding on the used + platform is as expected: if (!FRI_CHECK_SIZES_OK) ... */ +#define FRI_CHECK_SIZES_OK \ + ((sizeof(tFriHeader) == FRI_HEADER_SIZE) && \ + (sizeof(tFriKrlData) == FRI_KRL_DATA_SIZE) && \ + (sizeof(tFriIntfStatistics) == FRI_INTF_STATISTICS_SIZE) && \ + (sizeof(tFriIntfState) == FRI_INTF_STATE_SIZE) && \ + (sizeof(tFriRobotState) == FRI_ROBOT_STATE_SIZE) && \ + (sizeof(tFriRobotData) == FRI_ROBOT_DATA_SIZE) && \ + (sizeof(tFriRobotCommand) == FRI_ROBOT_COMMAND_SIZE) && \ + (sizeof(tFriMsrData) == FRI_MSR_DATA_SIZE) && \ + (sizeof(tFriCmdData) == FRI_CMD_DATA_SIZE)) + + + + +/** convenience macros for the user to check if byte order and float + representation on the used platform is as expected: + { + FRI_PREPARE_CHECK_BYTE_ORDER + if (!FRI_CHECK_BYTE_ORDER_OK) ... + }*/ +#define FRI_PREPARE_CHECK_BYTE_ORDER \ + union {fri_uint32_t a; fri_uint8_t b[4]; fri_float_t c;} _friTestByteOrder; \ + _friTestByteOrder.a = 0x40490FDB; + /** convenience macros for the user to check if byte order and float + representation on the used platform is as expected: + { + FRI_PREPARE_CHECK_BYTE_ORDER + if (!FRI_CHECK_BYTE_ORDER_OK) ... + }*/ +#define FRI_CHECK_BYTE_ORDER_OK \ + ((_friTestByteOrder.a == 0x40490FDB) &&\ + (_friTestByteOrder.b[0] == 0xDB) &&\ + (_friTestByteOrder.b[1] == 0x0F) &&\ + (_friTestByteOrder.b[2] == 0x49) &&\ + (_friTestByteOrder.b[3] == 0x40) &&\ + (_friTestByteOrder.c > 3.141592) &&\ + (_friTestByteOrder.c < 3.141593)) + + +/**! Versioning Major Version */ +#define FRI_MAJOR_VERSION 1 +/**! Versioning Minor sub version for minor fixes */ +#define FRI_SUB_VERSION 0 +#define EXPAND_CONCAT(X1,X2,X3,X4,X5,X6,X7) X1 ## X2 ## X3 ## X4 ## X5 ## X6 ## X7 +#define CONCAT(name1,name2,name3,name4) EXPAND_CONCAT(#name1,".",#name2,".",#name3,".",#name4) + +/**! The Versionstring with all significant information */ +#define FRI_VERSION_STRING CONCAT(FRI_MAJOR_VERSION, FRI_SUB_VERSION,FRI_DATAGRAM_ID_CMD,FRI_DATAGRAM_ID_MSR) +//"." #F "." #FRI_DATAGRAM_ID_MSR + +#endif /* FRICOMM_H */ diff --git a/lwrserv/friremote.cpp b/lwrserv/friremote.cpp new file mode 100755 index 0000000..8dd217b --- /dev/null +++ b/lwrserv/friremote.cpp @@ -0,0 +1,288 @@ +/*{{[PH] +**************************************************************************** + Project: FRI + + This material is the exclusive property of KUKA Roboter GmbH + and must be returned to KUKA Roboter GmbH immediately upon + request. This material and the information illustrated or + contained herein may not be used, reproduced, stored in a + retrieval system, or transmitted in whole or in part in any + way - electronic, mechanical, photocopying, recording, or + otherwise, without the prior written consent of KUKA Roboter GmbH. + + All Rights Reserved + Copyright (C) 2009 + KUKA Roboter GmbH + Augsburg, Germany + +[PH]}} +*/ + +/* +{{[FH] +**************************************************************************** + friRemote.cpp + + NOTE: This sample, as the corresponding FRI (Fast Research inteface) is subject to radical change + + +[FH]}} +*/ +/** + \addtogroup friRemoteLib + \brief Library for FRI (FastResearchInterface) +*/ +/* @{ */ + +/** ************************************************************************* + \author (Guenter Schreiber) +\file +\brief FRI Remote class encapsulating UDP package handshakes + * * + ***************************************************************************/ +#include "friremote.h" + +#include +#include + +friRemote::friRemote(int port,char *hintToRemoteHost):remote(port,hintToRemoteHost) +{ + //std::cout << __FUNCTION__ << " " < the respective cmd.cmd.cmdFlags field is set properly + Note: It is possible to change cmd.cmd.cmdFlags in monitor mode only !! + */ +int friRemote::doJntImpedanceControl(const float newJntPosition[LBR_MNJ], + const float newJntStiff[LBR_MNJ], + const float newJntDamp[LBR_MNJ], + const float newJntAddTorque[LBR_MNJ], + bool flagDataExchange) + +{ + // Helper, if not properly initialized or the like... + cmd.cmd.cmdFlags=0; + if (newJntPosition) + { + cmd.cmd.cmdFlags|=FRI_CMD_JNTPOS; + // Note:: If the interface is not in Command mode, + // The commands have to be "mirrored" to get in sync + if ((getState() != FRI_STATE_CMD) || (!isPowerOn())) + { + for (int i = 0; i < LBR_MNJ; i++) + { + cmd.cmd.jntPos[i]=msr.data.cmdJntPos[i]+msr.data.cmdJntPosFriOffset[i]; + } + } + else + { + // compute new values here ... + for (int i = 0; i < LBR_MNJ; i++) + cmd.cmd.jntPos[i]=newJntPosition[i]; + } + } + if (newJntStiff) + { + cmd.cmd.cmdFlags|=FRI_CMD_JNTSTIFF; + for (int i = 0; i < LBR_MNJ; i++) + cmd.cmd.jntStiffness[i]=newJntStiff[i]; + } + if (newJntDamp) + { + cmd.cmd.cmdFlags|=FRI_CMD_JNTDAMP; + for (int i = 0; i < LBR_MNJ; i++) + cmd.cmd.jntDamping[i]=newJntDamp[i]; + } + if (newJntAddTorque) + { + cmd.cmd.cmdFlags|=FRI_CMD_JNTTRQ; + for (int i = 0; i < LBR_MNJ; i++) + cmd.cmd.addJntTrq[i]=newJntAddTorque[i]; + } + + if (flagDataExchange) + { + return doDataExchange(); + } + return 1; +} + + /** automatically do data exchange, if not otherwise specified + if flagDataExchange is set to false, call doDataExchange() + or doReceiveData()/doSendData() on your own + IN: newJntPosition - joint positions + newJntStiff - joint stiffness (Spring factor) + newJntDamp - joint damping (Damping factor) + newJntAddTorque - additional torque + + Note: If any of the pointers (newJntPosition, newJntStiff, newJntDamp, newJntAddTorque) is NULL, the + value is ignored -> the respective cmd.cmd.cmdFlags field is set properly + Note: It is possible to change cmd.cmd.cmdFlags in monitor mode only !! + */ +int friRemote::doCartesianImpedanceControl(const float newCartPosition[FRI_CART_FRM_DIM], + const float newCartStiff[FRI_CART_VEC], + const float newCartDamp[FRI_CART_VEC], + const float newAddTcpFT[FRI_CART_VEC], + const float newJntNullspace[LBR_MNJ], + bool flagDataExchange) +{ + + // Helper, if not properly initialized or the like... + cmd.cmd.cmdFlags=0; + if ( newCartPosition ) + { + cmd.cmd.cmdFlags|=FRI_CMD_CARTPOS; + for ( int i = 0; i < FRI_CART_FRM_DIM; i++) + { + cmd.cmd.cartPos[i]=newCartPosition[i]; + + } + } + if ( newCartStiff) + { + cmd.cmd.cmdFlags|=FRI_CMD_CARTSTIFF; + for ( int i = 0; i < FRI_CART_VEC; i++) + { + cmd.cmd.cartStiffness[i]=newCartStiff[i]; + + } + + } + if ( newCartDamp) + { + cmd.cmd.cmdFlags|=FRI_CMD_CARTDAMP; + ; + for ( int i = 0; i < FRI_CART_VEC; i++) + { + cmd.cmd.cartDamping[i]=newCartDamp[i]; + + } + } + if ( newAddTcpFT) + { + cmd.cmd.cmdFlags|=FRI_CMD_TCPFT; + ; + for ( int i = 0; i < FRI_CART_VEC; i++) + { + cmd.cmd.addTcpFT[i]=newAddTcpFT[i]; + + } + } + + if (newJntNullspace) + { + cmd.cmd.cmdFlags|=FRI_CMD_JNTPOS; + // Note:: If the interface is not in Command mode, + // The commands have to be "mirrored" to get in sync + + // compute new values here ... + for (int i = 0; i < LBR_MNJ; i++) + cmd.cmd.jntPos[i]=newJntNullspace[i]; + } + + +if (flagDataExchange) + { + return doDataExchange(); + } + return 1; +} + + + + + + +/* @} */ + diff --git a/lwrserv/friremote.h b/lwrserv/friremote.h new file mode 100755 index 0000000..c3d5656 --- /dev/null +++ b/lwrserv/friremote.h @@ -0,0 +1,194 @@ +/* {{[PH] +**************************************************************************** + Project: FRI + + This material is the exclusive property of KUKA Roboter GmbH + and must be returned to KUKA Roboter GmbH immediately upon + request. This material and the information illustrated or + contained herein may not be used, reproduced, stored in a + retrieval system, or transmitted in whole or in part in any + way - electronic, mechanical, photocopying, recording, or + otherwise, without the prior written consent of KUKA Roboter GmbH. + + All Rights Reserved + Copyright (C) 2009 + KUKA Roboter GmbH + Augsburg, Germany + +[PH]}} +*/ + +/* +{{[FH] +**************************************************************************** + friFirstApp.cpp + + NOTE: This sample, as the corresponding FRI (Fast Research inteface) is subject to radical change + + +[FH]}} +*/ +/** + \defgroup friRemoteLib + \brief Library for FRI (FastResearchInterface) +*/ +/* @{ */ + +/** + \author (Guenter Schreiber) +\file +\brief Remote class for handshaking/dealing with udp datastructures + + This code is most important to understand the concepts behind data handshake +*/ +#ifndef FRIFRIREMOTE_H +#define FRIFRIREMOTE_H + + +#include "friudp.h" + + + + +/** + @author Günter Schreiber +*/ + +class friRemote +{ +public: + friRemote(int port = FRI_DEFAULT_SERVER_PORT, char * hintToRemoteHost=NULL); + ~friRemote(); + + /** Data Exchanger -- normally update within access routine implicitely + send commands based on last datagram and after waits on new measurement + calls doSendData() and doReceiveData(); + ... */ + int doDataExchange(); + /** Receives data while calling friUdp::Recv() + The call will block.. + */ + int doReceiveData(); + /** Sends the data */ + int doSendData(); + /* @{ */ + /** automatically do data exchange, if not otherwise specified + if flagDataExchange is set to false, call doDataExchange() + or doReceiveData()/doSendData() on your own + */ + int doPositionControl(float newJntPosition[LBR_MNJ], bool flagDataExchange=true); + + /** automatically do data exchange, if not otherwise specified + if flagDataExchange is set to false, call doDataExchange() + or doReceiveData()/doSendData() on your own + IN: newJntPosition - joint positions + newJntStiff - joint stiffness (Spring factor) + newJntDamp - joint damping (Damping factor) + newJntAddTorque - additional torque + + Note: If any of the pointers (newJntPosition, newJntStiff, newJntDamp, newJntAddTorque) is NULL, the + value is ignored -> the respective cmd.cmd.cmdFlags field is set properly + Note: It is possible to change cmd.cmd.cmdFlags in monitor mode only !! + */ + int doJntImpedanceControl(const float newJntPosition[LBR_MNJ], const float newJntStiff[LBR_MNJ] = NULL, const float newJntDamp[LBR_MNJ]=NULL, const float newJntAddTorque[LBR_MNJ]=NULL,bool flagDataExchange=true); + + + /** automatically do data exchange, if not otherwise specified + if flagDataExchange is set to false, call doDataExchange() + or doReceiveData()/doSendData() on your own + IN: newJntPosition - joint positions + newJntStiff - joint stiffness (Spring factor) + newJntDamp - joint damping (Damping factor) + newJntAddTorque - additional torque + + Note: If any of the pointers (newJntPosition, newJntStiff, newJntDamp, newJntAddTorque) is NULL, the + value is ignored -> the respective cmd.cmd.cmdFlags field is set properly + Note: It is possible to change cmd.cmd.cmdFlags in monitor mode only !! + */ + int doCartesianImpedanceControl(const float newCartPosition[FRI_CART_FRM_DIM], + const float newCartStiff[FRI_CART_VEC]=NULL, + const float newCartDamp[FRI_CART_VEC]=NULL, + const float newAddTcpFT[FRI_CART_VEC]=NULL, + const float newJntNullspace[LBR_MNJ]=NULL, bool flagDataExchange=true); + /* @} */ + + /** measured Cartesian frame (in m) + KRL: $POS_ACT_MSR + Reference: Base and tool are specified by $stiffness.base, $stiffness.tool + */ + float * getMsrCartPosition() { return msr.data.msrCartPos; } + /** commanded Cartesian frame (in m, before FRI) + KRL: $POS_ACT_CMD + Reference: Base and tool are specified by $stiffness.base, $stiffness.tool + */ + float * getMsrCmdCartPosition() { return msr.data.cmdCartPos; } + + /** commanded Cartesian frame (in m, due to FRI) */ + float * getMsrCmdCartPosFriOffset() { return msr.data.cmdCartPosFriOffset; } + + /** Access to inner buffers for further manipulation */ + tFriMsrData & getMsrBuf() { return msr;} + tFriCmdData & getCmdBuf() { return cmd;} + /* @{ */ + /** interpretational access routines */ + FRI_STATE getState() { return (FRI_STATE)msr.intf.state; } + FRI_QUALITY getQuality() { return (FRI_QUALITY)msr.intf.quality; } + FRI_CTRL getCurrentControlScheme (){ return (FRI_CTRL)msr.robot.control; } + + bool isPowerOn() { return msr.robot.power!=0; } + /* @} */ + + /* @{ */ + /** Important value for superposition - and during poweroff stages, to become command mode */ + float * getMsrCmdJntPosition() { return msr.data.cmdJntPos; } + /** returns the offset, which is commanded by FRI Remote side + * Complete desired position inside LBR Kernel is the sum of cmdJntPos and cmdJntPosFriOffset */ + float * getMsrCmdJntPositionOffset() { return msr.data.cmdJntPosFriOffset; } + + void getCurrentCmdJntPosition( float jntVec[LBR_MNJ] ) { for ( int i = 0; i < LBR_MNJ; i++) jntVec[i]= msr.data.cmdJntPos[i]+msr.data.cmdJntPosFriOffset[i];} + /** Current measured jnt position of the robot */ + float * getMsrMsrJntPosition() { return msr.data.msrJntPos; } + float * getMsrEstExtJntTrq() { return msr.data.estExtJntTrq; } + float * getMsrJntTrq() { return msr.data.msrJntTrq; } + /* @} */ + + float getSampleTime() { return msr.intf.desiredCmdSampleTime; } + int getSequenceCount() { return seqCount; } + + /* @{ */ + /** KRL Interaction -- Corresponds to $FRI_TO_REA */ + float getFrmKRLReal(int index) { return msr.krl.realData[index]; } + /** KRL Interaction -- Corresponds to $FRI_FRM_REA */ + void setToKRLReal(int index, float val) { cmd.krl.realData[index]=val; } + /** KRL Interaction -- Corresponds to $FRI_TO_INT */ + int getFrmKRLInt(int index) { return msr.krl.intData[index]; } + /** KRL Interaction -- Corresponds to $FRI_FRM_INT */ + void setToKRLInt(int index, int val) { cmd.krl.intData[index]=val; } + /** KRL Interaction -- Corresponds to $FRI_TO_BOOL */ + bool getFrmKRLBool(int index) { return ((msr.krl.boolData & (1<h_addr_list[i]!=0; i++) + { + struct in_addr addr; + memcpy(&addr, host->h_addr_list[i], sizeof(addr)); + //printf("helloIP %s - Port %d\n", inet_ntoa(addr), serverPort); + } + } +#endif // + + +} + +/* reveive one packet from KRC (blocking!) */ +int friUdp::Recv(tFriMsrData *packet) +{ + if (udpSock >= 0) + { + int received; + struct timeval ts; + + received = RecvPacket(udpSock, packet, &ts, &krcAddr); + + if (received == sizeof(tFriMsrData)) + { +#ifdef HAVE_TIME_STAMP_RECEIVE + + /* FIXME: need another #ifdef for VxWorks */ +#ifdef QNX + struct timespec ts; + clock_gettime(CLOCK_REALTIME, &ts); + m_timestamp = (double)ts.tv_sec + (double)ts.tv_nsec/1.0e9; +#else + m_timestamp = (double)ts.tv_sec + (double)ts.tv_usec/1.0e6; +#endif // QNX +#endif // HAVE_TIME_STAMP + return 0; + } + else + { + printf("received something, but wrong size %d (expected %d)...\n",received,(int) sizeof(tFriMsrData)); + fflush(stdout); + } + } + memset(packet, 0, sizeof(tFriMsrData)); + return -1; +} + + + +/* send one answer packet to KRC */ +int friUdp::Send(tFriCmdData *data) +{ + krcAddr.sin_family = AF_INET; +#ifdef KRC_IP_ADDRESS + krcAddr.sin_addr.s_addr = inet_addr(KRC_IP_ADDRESS); +#endif +#ifdef KRC_RECEIVE_PORT + krcAddr.sin_port = htons(KRC_RECEIVE_PORT); +#endif + + if ((udpSock >= 0) && (ntohs(krcAddr.sin_port) != 0)) + { + int sent; + sent = sendto(udpSock, (char *) data, sizeof(tFriCmdData), 0, + (struct sockaddr *)&krcAddr, sizeof(krcAddr)); + if (sent == sizeof(tFriCmdData)) + { + return 0; + } + } + return -1; +} + + + + +/* close the socket */ +void friUdp::Close(void) +{ + if (udpSock >= 0) + { +#ifdef WIN32 + closesocket(udpSock); + WSACleanup(); +#else + close(udpSock); +#endif + } + udpSock = -1; +} + + +#ifdef HAVE_TIME_STAMP_RECEIVE +// Socket option SO_TIMESTAMP is supported +/* receive with timestamp */ +int friUdp::RecvPacket(int fd, tFriMsrData* p, struct timeval* ts, struct sockaddr_in* client) +{ + struct msghdr msg; + struct iovec vec[1]; + union { + struct cmsghdr cm; + char control[20]; + } cmsg_un; + struct cmsghdr *cmsg; + struct timeval *tv = NULL; + int n; + + vec[0].iov_base = p; + vec[0].iov_len = sizeof(*p); + + memset(&msg, 0, sizeof(msg)); + memset(&cmsg_un, 0, sizeof(cmsg_un)); + + msg.msg_name = (caddr_t)client; + if(client) + msg.msg_namelen = sizeof(*client); + else + msg.msg_namelen = 0; + + msg.msg_iov = vec; + msg.msg_iovlen = 1; + msg.msg_control = cmsg_un.control; + msg.msg_controllen = sizeof(cmsg_un.control); + msg.msg_flags = 0; + + n = recvmsg(fd, &msg, 0); // MSG_DONTWAIT + if(n < 0) + { + perror("recvmsg"); + return -1; + } + if(msg.msg_flags & MSG_TRUNC) + { + printf("received truncated message\n"); + return -1; + } + if(!ts) + return n; + + /* get time stamp of packet */ + if(msg.msg_flags & MSG_CTRUNC) + { + printf("received truncated ancillary data\n"); + return -1; + } + if(msg.msg_controllen < sizeof(cmsg_un.control)) + { + printf("received short ancillary data (%d/%d)\n", msg.msg_controllen, (int)sizeof(cmsg_un.control)); + return -1; + } + for(cmsg = CMSG_FIRSTHDR(&msg); cmsg != NULL; cmsg = CMSG_NXTHDR(&msg, cmsg)) + { + if (cmsg->cmsg_level == SOL_SOCKET && cmsg->cmsg_type == SCM_TIMESTAMP) + tv = (struct timeval *)CMSG_DATA(cmsg); + } + if(tv) + { + ts->tv_sec = tv->tv_sec; + ts->tv_usec = tv->tv_usec; + } + return n; +} + +#else +/* receive with timestamp */ +int friUdp::RecvPacket(int udpSock, tFriMsrData* data, struct timeval* ts, struct sockaddr_in* client) +{ + + if (udpSock >= 0) + { + /** HAVE_SOCKLEN_T + Yes - unbelieavble: There are differences in standard calling parameters (types) to recvfrom + Windows winsock, VxWorks and QNX use int + newer Posix (most Linuxes) use socklen_t + */ + +#ifdef HAVE_SOCKLEN_T + socklen_t sockAddrSize; +#else + int sockAddrSize; +#endif + int received; + + sockAddrSize = sizeof(struct sockaddr_in); + + received = recvfrom(udpSock, (char *) data, sizeof(tFriMsrData), 0, + (struct sockaddr *)&krcAddr, &sockAddrSize); + + return received; + } + return -1; +} + +#endif // HAVE_TIME_STAMP_RECEIVE + + +#ifdef VXWORKS //USE_BERKELEY_PACKAGE_FILTER_VXWORKS +#define DEBUG_BPF_READ + +#include "vxworks.h" +#include "bpfDrv.h" +#include "ioLib.h" +#include +#include +//#include "drv/netif/smNetLib.h" +#include +#include +#include +#include + +#ifdef DEBUG_BPF_READ +#include +#include "friremote.h" + +#endif + + +/* + * Packet filter program... + * + * XXX: Changes to the filter program may require changes to the + * constant offsets used in if_register_send to patch the BPF program! + */ +struct bpf_insn friUpdSock_bpf_filter[] = +{ + /* Make sure this is an IP packet... */ + BPF_STMT(BPF_LD + BPF_H + BPF_ABS, 12), + BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, ETHERTYPE_IP, 0, 8), + + /* Make sure it's a UDP packet... */ + BPF_STMT(BPF_LD + BPF_B + BPF_ABS, 23), + BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, IPPROTO_UDP, 0, 6), + + /* Make sure this isn't a fragment... */ + BPF_STMT(BPF_LD + BPF_H + BPF_ABS, 20), + BPF_JUMP(BPF_JMP + BPF_JSET + BPF_K, 0x1fff, 4, 0), + + /* Get the IP header length... */ + BPF_STMT(BPF_LDX + BPF_B + BPF_MSH, 14), + + /* Make sure it's to the right port... */ + BPF_STMT(BPF_LD + BPF_H + BPF_IND, 16), + BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, 67, 0, 1), /* patch */ + + /* If we passed all the tests, ask for the whole packet. */ + BPF_STMT(BPF_RET+BPF_K, (u_int)-1), + + /* Otherwise, drop it. */ + BPF_STMT(BPF_RET+BPF_K, 0), +}; + +int friUpdSock_bpf_filter_len = sizeof(friUpdSock_bpf_filter) / sizeof(struct bpf_insn); +struct bpf_program mybpf; + + +void testBPF1(int socketPort, char * devName) +{ + int bpffd = 0; + struct bpf_hdr * buf = NULL; + char * pbuffer = NULL; + int buflen; + int len,i; + ///int j=10; + char dev[8] = "gei0"; + struct ifreq ifr; + int trueValue=1; + int Rcvlen; + + if ( socketPort <= 10) + socketPort = 12345; + + if ( devName != NULL ) + { + strncpy(dev,devName,8); + dev[8]=0; + } + mybpf.bf_len = friUpdSock_bpf_filter_len; + mybpf.bf_insns = friUpdSock_bpf_filter; + + /* Patch the server port into the BPF program... + * + * XXX: changes to filter program may require changes to the + * insn number(s) used below! + */ + friUpdSock_bpf_filter[8].k = socketPort; + + bpfDrv(); + + if ( bpfDevCreate("/dev/bpf",2,4096) == ERROR) + { + printf("bpfDevCreate failed \n"); + return; + } + bpffd = open( "/dev/bpf0",0,0); + if ( bpffd <= 0) + { + printf("open /dev/bpf0 failed\n"); + return; + } + + memset(&ifr, sizeof(struct ifreq), 0); + strncpy(ifr.ifr_name, dev, sizeof(ifr.ifr_name)); + +#define IOCTRL_CAST_THIRD_ARG (int) + + if (ioctl(bpffd,BIOCIMMEDIATE, IOCTRL_CAST_THIRD_ARG &trueValue) < 0) + { + printf("Error BIOCIMMEDIATE \n"); + } + + if (ioctl(bpffd,(BIOCSETF),IOCTRL_CAST_THIRD_ARG (caddr_t)(&mybpf)) < 0) + { + perror("Error BIOCSETF \n"); + goto errorMark; + } + + if (ioctl(bpffd,(BIOCSETIF),IOCTRL_CAST_THIRD_ARG (caddr_t)&ifr) < 0) + { + printf("ERROR BIOCSETIF %s \n",dev); + goto errorMark; + } + + if (ioctl(bpffd,BIOCGBLEN, IOCTRL_CAST_THIRD_ARG &buflen) < 0) + { + printf("Error BIOCGBLEN \n"); + } + + if (buflen > 4096) + buflen=4096; + + buf = (struct bpf_hdr *)malloc(buflen); + //bzero(buf,buflen); + memset(buf,0x0,buflen); + + while ((len = read(bpffd,(char *)buf,buflen)) != 0) + { + // im bpf header steht noch ein Timestamp -- waere gut fuer Timing thematik + // + + // Empfangene Rohdaten ohne bpf Header + pbuffer = (char *)buf + buf->bh_hdrlen; + + // Empfangene Rohdatenlaenge ohne bpf Header + Rcvlen = len - (buf->bh_hdrlen); + // + // Wie trennt man nun die "Nutzdaten" von den Verwaltungsdaten?? + // + + struct ip * iph = (struct ip *) ((char *) buf + buf->bh_hdrlen + sizeof(struct ether_header)); + struct udphdr * udph = (struct udphdr *) ((char *) iph + sizeof(struct ip)); + char * userData = ((char *) udph) + sizeof( struct udphdr); + + tFriCmdData * cmd = ( tFriCmdData * ) userData; +#ifdef DEBUG_BPF_READ + printf("recvLen %d\n",Rcvlen); + printf("IP SRC:\t\t%s\n", inet_ntoa(iph->ip_src)); + printf("IP DST:\t\t%s\n", inet_ntoa(iph->ip_dst)); + printf("UDP SRC:\t%u\n", ntohs(udph->uh_sport)); + printf("UDP DST:\t%u\n", ntohs(udph->uh_dport)); + printf("Len #:\t\t%u\n", ntohs(udph->uh_ulen)); +#endif + + logMsg("BUF LEN = 0x%x\n",Rcvlen,0,0,0,0,0); + + std::cout << (*cmd) << std::endl; + for (i=0;i< ntohs(udph->uh_ulen);i++) + { + printf(" %2x", userData[i]); + if ((i % 10) == 9) + { + printf("\n");//,0,0,0,0,0,0); + } + } + printf("\nFull packet \n"); + for (i=0; i < Rcvlen; i++) + { + printf(" %2x(%4d)", pbuffer[i],pbuffer[i]); + if ((i % 10) == 9) + { + printf("\n");//,0,0,0,0,0,0); + } + } + } +errorMark: + printf("leaving %s\n",__PRETTY_FUNCTION__); + if ( buf != NULL ) + free(buf); + if ( bpffd > 0 ) + close (bpffd); + bpfDevDelete("/dev/bpf"); +} +#endif + + +/***************************************************************************** + $Log: $ + *****************************************************************************/ +/* @} */ diff --git a/lwrserv/friudp.h b/lwrserv/friudp.h new file mode 100755 index 0000000..fd54511 --- /dev/null +++ b/lwrserv/friudp.h @@ -0,0 +1,257 @@ +/*{{[PH] +**************************************************************************** + Project: FRI + + This material is the exclusive property of KUKA Roboter GmbH + and must be returned to KUKA Roboter GmbH immediately upon + request. This material and the information illustrated or + contained herein may not be used, reproduced, stored in a + retrieval system, or transmitted in whole or in part in any + way - electronic, mechanical, photocopying, recording, or + otherwise, without the prior written consent of KUKA Roboter GmbH. + + All Rights Reserved + Copyright (C) 2009 + KUKA Roboter GmbH + Augsburg, Germany + +[PH]}} +*/ + +/* +{{[FH] +**************************************************************************** + friUdp.h + + NOTE: This sample, as the corresponding FRI (Fast Research inteface) is subject to radical change + + +[FH]}} +*/ +/** + \addtogroup friRemoteLib + \brief Library for FRI (FastResearchInterface) +*/ +/* @{ */ +/** ************************************************************************* + \author (Guenter Schreiber) +\file +\brief header for udp Communications + * * + ***************************************************************************/ +#ifndef FRIFRIUDP_H +#define FRIFRIUDP_H + +#ifdef VXWORKS +#else +#define HAVE_GETHOSTNAME +#endif + +#include +#include + +#ifdef _MSC_VER +#include "SocketObject.h" +#else +#include +#include +#include +#include +#include +#include +#ifndef _WRS_KERNEL +#include +#endif +#include +#endif +#include "friComm.h" + +#include + +#ifdef VXWORKS // VxWorks Kernel + +#include +#endif + + +#define FRI_DEFAULT_SERVER_PORT 49938 + +#ifdef QNX +#define HAVE_TIME_STAMP_RECEIVE +#endif + + +#ifdef HAVE_TIME_STAMP_RECEIVE +/** Receive Timestamp -- mechanism works under +QNX, Linux? + +Not under Windows - and under VxWorks +*/ + +typedef struct +{ + tFriMsrData load; + /* add a received-timestamp */ + double timestamp; +} tFriMsrPacket; + +#endif + +/** +FRI Remote Sample Implementation + + @author Günter Schreiber +*/ +class friUdp +{ +public: + friUdp(int port=FRI_DEFAULT_SERVER_PORT, char *remoteHost = NULL); + ~friUdp(); + +protected: + /// Note: Remote host need not to be specified - if NULL, wait for the + /// incoming packages + void Init(char * remoteHost=NULL); + void Close(void); +#ifdef WIN32 + int StartWinsock(void); +#endif +public: + int Send(tFriCmdData *data); + int Recv(tFriMsrData *packet); +private: + int RecvPacket(int fd, tFriMsrData* p, struct timeval* ts, struct sockaddr_in* client); + int udpSock ; + int serverPort; + struct sockaddr_in krcAddr; + /// if timestamp on receive is available, last received value can be inquired here + double m_timestamp; +public: + /// This feature will be available only for systems, which support + /// SO_TIMESTAMP in the socket options, e.g. qnx + double getLastTimestamp() { return m_timestamp; } +}; + + + + +inline std::ostream & operator<<(std::ostream &out , tFriHeader & head) +{ + out << "sendSeqCount " << head.sendSeqCount << "\n"; + out << "reflSeqCount " << head.reflSeqCount << "\n"; + out << "packetSize " << head.packetSize << "\n"; + out << "datagramId " << std::hex << head.datagramId << std::dec ; + switch (head.datagramId ) + { + case FRI_DATAGRAM_ID_CMD: + out << " FRI_DATAGRAM_ID_CMD \n" ; + break; + case FRI_DATAGRAM_ID_MSR: + out << " FRI_DATAGRAM_ID_MSR \n" ; + break; + default: + out <<" Unkown \n"; + } + return out; +} +inline std::ostream & operator<<(std::ostream &out , tFriKrlData& krl) +{ + out << "krl_real "; + for ( int i = 0; i < FRI_USER_SIZE; i++) + out << " " << krl.realData[i]; + out << "\n"; + out << "krl_int "; + for ( int i = 0; i < FRI_USER_SIZE; i++) + out << " " << krl.intData[i]; + out << "\n"; + out << "krl_bool "; + out << std::hex << krl.boolData << std::dec << "\n"; + return out; +} + +inline std::ostream & operator<<(std::ostream &out , tFriIntfStatistics & stat) +{ + out << "answerRate " << stat.answerRate << "\n"; + out << "latency " << stat.latency << "\n"; + out << "jitter " << stat.jitter << "\n"; + out << "missRate " << stat.missRate << "\n"; + out << "missCounter " << stat.missCounter << "\n"; + return out; +} + +inline std::ostream & operator<<(std::ostream &out , tFriIntfState & state) +{ + out << "timestamp " << state.timestamp<< "\n"; + out << "state " << state.state<< "\n"; + out << "quality " << state.quality << "\n"; + out << "desiredMsrSampleTime " << state.desiredMsrSampleTime << "\n"; + out << "desiredCmdSampleTiintfme " << state.desiredCmdSampleTime << "\n"; + out << "safetyLimits " << state.safetyLimits << "\n"; + out << "statistics " << state.stat << "\n"; +return out; +} + +inline std::ostream & operator<< (std::ostream & out, tFriRobotState & robot) +{ + out << "power " << robot.power<< "\n"; + out << "control " << robot.control << "\n"; + out << "error " << robot.error << "\n"; + out << "warning " << robot.warning << "\n"; + out << "temperature " ; + for (int i = 0; i < LBR_MNJ; i++) + out << robot.temperature[i] << " " ; + out << "\n"; + return out; + +} + +#define WRITE_JNT_VEC(field) \ + out << ""#field; \ + for ( int i = 0; i < LBR_MNJ; i++) \ + out << " " << robot.field[i]; \ + out << "\n"; +inline std::ostream & operator<<(std::ostream &out, tFriRobotData & robot) +{ + WRITE_JNT_VEC(msrJntPos); + WRITE_JNT_VEC(cmdJntPos); + WRITE_JNT_VEC(msrJntTrq); + WRITE_JNT_VEC(estExtJntTrq); + return out; + +} + +inline std::ostream & operator<<(std::ostream &out, tFriRobotCommand & robot) +{ + out << std::hex << robot.cmdFlags << std::dec << "\n"; + WRITE_JNT_VEC(jntPos); + return out; + +} + + +inline std::ostream & operator<<(std::ostream &out, tFriMsrData & msr) +{ + out << "head " << msr.head; + out << "krl " << msr.krl; + out << "intf " << msr.intf; + out << "robot " << msr.robot; + out << "data " << msr.data; + return out; +} + + + + +inline std::ostream & operator<<(std::ostream &out, tFriCmdData & cmd) +{ + out << "head " << cmd.head; + out << "krl " << cmd.krl; + out << "cmd " << cmd.cmd; + return out; +} + + + + +#endif +/* @} */ diff --git a/lwrserv/global.cpp b/lwrserv/global.cpp new file mode 100755 index 0000000..e9f5677 --- /dev/null +++ b/lwrserv/global.cpp @@ -0,0 +1,36 @@ +bool __SVR_CURRENT_JOB = false; +bool __MSRMSRJNTPOS = false; +bool __MSRCMDJNTPOS = false; +bool __MSRCMDPOSE = false; +bool __SETVELOCITY = false; +bool __SETACCEL = false; +bool __DEBUG = false; +bool __POTFIELDMODE = false; +bool __MSRSTARTPOTFIELD = false; +bool __MSRSTOPPOTFIELD = false; +bool __MSRSETPOS = false; +bool __CARTMOVE = false; +bool __DOUS = false; +bool __DOUS2 = false; + +int globi = 0; + +float MSRMSRJNTPOS[7]; +double MSRCMDJNTPOS[7]; +double MSRMSRCARTPOS[12]; +float MSRCMDCARTPOS[12]; +double MSRMSRJACOBIAN[42]; +double MSRMSRFTTCP[6]; +double MSRCMDPOSE[3][4]; +float USTarget[12]; +float USApproach[12]; +double VELOCITY = 20; +double ACCELARATION = 10; +double maxVelJnt[] = {110.0,110.0,128.0,128.0,204.0,184.0,184.0}; +double maxAccJnt[] = {1.0,1.0,1.0,1.0,2.0,1.0,1.0}; +double maxTrqJnt[] = {176.0,176.0,100.0,100.0,100.0,38.0,38.0}; +double maxRangeJnt[] = {170.0,120.0,170.0,120.0,170.0,120.0,170.0}; + + + + diff --git a/lwrserv/global.h b/lwrserv/global.h new file mode 100755 index 0000000..3b0c854 --- /dev/null +++ b/lwrserv/global.h @@ -0,0 +1,32 @@ +extern bool __SVR_CURRENT_JOB; +extern bool __MSRMSRJNTPOS; +extern bool __MSRCMDJNTPOS; +extern bool __MSRCMDPOSE; +extern bool __SETVELOCITY; +extern bool __SETACCEL; +extern bool __DEBUG; +extern bool __POTFIELDMODE; +extern bool __MSRSTARTPOTFIELD; +extern bool __MSRSTOPPOTFIELD; +extern bool __MSRSETPOS; +extern bool __CARTMOVE; +extern bool __DOUS; +extern bool __DOUS2; + +extern int globi; + +extern float MSRMSRJNTPOS[7]; +extern double MSRMSRCARTPOS[12]; +extern float MSRCMDCARTPOS[12]; +extern double MSRMSRJACOBIAN[42]; +extern double MSRMSRFTTCP[6]; +extern double MSRCMDJNTPOS[7]; +extern double MSRCMDPOSE[3][4]; +extern float USTarget[12]; +extern float USApproach[12]; +extern double VELOCITY; +extern double ACCELARATION; +extern double maxVelJnt[LBR_MNJ]; +extern double maxAccJnt[LBR_MNJ]; +extern double maxTrqJnt[LBR_MNJ]; +extern double maxRangeJnt[LBR_MNJ]; \ No newline at end of file diff --git a/lwrserv/header.h b/lwrserv/header.h new file mode 100755 index 0000000..564af09 --- /dev/null +++ b/lwrserv/header.h @@ -0,0 +1,24 @@ +#include +#include +#include "defines.h" +#include "SocketObject.h" +#include "SvrHandling.h" +#include "StringTool.h" +#include +#include "pthread.h" +#include "friremote.h" +#include "math.h" +#include +#include "mat.h" +#include "vec.h" +//#include "opencv2\opencv.hpp" +//#pragma comment (lib, "opencv_core248d.lib") + + +//debug +#include +//end debug + +using namespace std; +//using namespace cv; + diff --git a/lwrserv/include/libblas.dll b/lwrserv/include/libblas.dll new file mode 100755 index 0000000..0ef85a0 Binary files /dev/null and b/lwrserv/include/libblas.dll differ diff --git a/lwrserv/include/libblas.lib b/lwrserv/include/libblas.lib new file mode 100755 index 0000000..6e3072a Binary files /dev/null and b/lwrserv/include/libblas.lib differ diff --git a/lwrserv/include/liblapack.dll b/lwrserv/include/liblapack.dll new file mode 100755 index 0000000..d16ca2b Binary files /dev/null and b/lwrserv/include/liblapack.dll differ diff --git a/lwrserv/include/liblapack.lib b/lwrserv/include/liblapack.lib new file mode 100755 index 0000000..5436f63 Binary files /dev/null and b/lwrserv/include/liblapack.lib differ diff --git a/lwrserv/mat.h b/lwrserv/mat.h new file mode 100644 index 0000000..dad28bb --- /dev/null +++ b/lwrserv/mat.h @@ -0,0 +1,354 @@ +#ifndef _MAT_H_ +#define _MAT_H_ +#include +#include "vec.h" + +template class CVector; + +// template square matrix class for SIMPLE data types +template class Mat +{ +public: + + Mat () + { + for (unsigned int j=0; j () + { + // nothing to do here ... + } + + Mat (const T aatData[SIZE][SIZE]) + { + for (unsigned int j=0; j (const Mat &mat) + { + for (unsigned int j=0; j=SIZE) + i = SIZE-1; + if (j>=SIZE) + j = SIZE-1; + return m_aatData[i][j]; + } + + T operator () (unsigned i, unsigned j) const + { + if (i>=SIZE) + i = SIZE-1; + if (j>=SIZE) + j = SIZE-1; + return m_aatData[i][j]; + } + + // CMatrixoperator + (const Matrix &mat) + // CMatrixoperator - (const Matrix &mat) + // ... + + Mat operator * (const Mat &mat) + { + Mat buf; + for (unsigned int i=0; i operator * (const Vec &vec) + { + Vec buf; + for (unsigned int j=0; j inv() + { + Mat invOut; + + long indxc[4], indxr[4], ipiv[4]; + long i, icol, irow, j, ir, ic; + long big, dum, pivinv,temp, bb; + + ipiv[0] = -1; + ipiv[1] = -1; + ipiv[2] = -1; + ipiv[3] = -1; + + invOut = *this; + + + for (i =0; i < 4 ; i++) { + big =0.0f; + for (j =0; j < 4 ; j++) { + if (ipiv[j] != 0) { + if (ipiv[0] == - 1) { + if ((bb = (float) fabs(invOut(j,0))) > big) { + big = bb; + irow = j; + icol = 0; + } + } else if (ipiv[0] > 0) { + goto end; + } + + if (ipiv[1] == - 1) { + if ((bb = (float) fabs(invOut(j,1))) > big) { + big = bb; + irow = j; + icol = 1; + } + } else if (ipiv[1] > 0) { + goto end; + } + + if (ipiv[2] == - 1) { + if ((bb = (float) fabs(invOut(j,2))) > big) { + big = bb; + irow = j; + icol = 2; + } + } else if (ipiv[2] > 0) { + goto end; + } + + if (ipiv[3] == - 1) { + if ((bb = (float) fabs(invOut(j,3))) > big) { + big = bb; + irow = j; + icol = 3; + } + } else if (ipiv[3] > 0) { + goto end; + } + } + } + ++(ipiv[icol]); + + if (irow != icol) { + temp = invOut(irow,0); + invOut(irow, 0) = invOut(icol, 0); + invOut(icol, 0) = temp; + + temp = invOut(irow,1); + invOut(irow, 1) = invOut(icol, 1); + invOut(icol,1) = temp; + + temp = invOut(irow,2); + invOut(irow,2) = invOut(icol,2); + invOut(icol,2) = temp; + + temp = invOut(irow,3); + invOut(irow,3) = invOut(icol,3); + invOut(icol,3) = temp; + } + + indxr[i] = irow; + indxc[i] = icol; + + if (invOut(icol,icol) ==0.0) { + goto end; + } + + pivinv =1.0f / invOut(icol,icol); + invOut(icol,icol) =1.0f; + invOut(icol,0) *= pivinv; + invOut(icol,1) *= pivinv; + invOut(icol,2) *= pivinv; + invOut(icol,3) *= pivinv; + + if (icol != 0) { + dum = invOut(0,icol); + invOut(0,icol) =0.0f; + invOut(0,0) -= invOut(icol,0) * dum; + invOut(0,1) -= invOut(icol,1) * dum; + invOut(0,2) -= invOut(icol,2) * dum; + invOut(0,3) -= invOut(icol,3) * dum; + } + + if (icol != 1) { + dum = invOut(1,icol); + invOut(1,icol) =0.0f; + invOut(1,0) -= invOut(icol,0) * dum; + invOut(1,1) -= invOut(icol,1) * dum; + invOut(1,2) -= invOut(icol,2) * dum; + invOut(1,3) -= invOut(icol,3) * dum; + } + + if (icol != 2) { + dum = invOut(2,icol); + invOut(2,icol) =0.0f; + invOut(2,0) -= invOut(icol,0) * dum; + invOut(2,1) -= invOut(icol,1) * dum; + invOut(2,2) -= invOut(icol,2) * dum; + invOut(2,3) -= invOut(icol,3) * dum; + } + + if (icol != 3) { + dum = invOut(3,icol); + invOut(3,icol) =0.0f; + invOut(3,0) -= invOut(icol,0) * dum; + invOut(3,1) -= invOut(icol,1) * dum; + invOut(3,2) -= invOut(icol,2) * dum; + invOut(3,3) -= invOut(icol,3) * dum; + } + } + if (indxr[3] != indxc[3]) { + ir = indxr[3]; + ic = indxc[3]; + temp = invOut(0,ir); + invOut(0,ir) = invOut(0,ic); + invOut(0,ic) = temp; + temp = invOut(1,ir); + invOut(1,ir) = invOut(1,ic); + invOut(1,ic) = temp; + temp = invOut(2,ir); + invOut(2,ir) = invOut(2,ic); + invOut(2,ic) = temp; + temp = invOut(3,ir); + invOut(3,ir) = invOut(3,ic); + invOut(3,ic) = temp; + } + if (indxr[2] != indxc[2]) { + ir = indxr[2]; + ic = indxc[2]; + + temp = invOut(0,ir); + invOut(0,ir) = invOut(0,ic); + invOut(0,ic) = temp; + + temp = invOut(1,ir); + invOut(1,ir) = invOut(1,ic); + invOut(1,ic) = temp; + + temp = invOut(2,ir); + invOut(2,ir) = invOut(2,ic); + invOut(2,ic) = temp; + + temp = invOut(3,ir); + invOut(3,ir) = invOut(3,ic); + invOut(3,ic) = temp; + } + if (indxr[1] != indxc[1]) { + ir = indxr[1]; + ic = indxc[1]; + temp = invOut(0,ir); + invOut(0,ir) = invOut(0,ic); + invOut(0,ic) = temp; + temp = invOut(1,ir); + invOut(1,ir) = invOut(1,ic); + invOut(1,ic) = temp; + temp = invOut(2,ir); + invOut(2,ir) = invOut(2,ic); + invOut(2,ic) = temp; + temp = invOut(3,ir); + invOut(3,ir) = invOut(3,ic); + invOut(3,ic) = temp; + } + if (indxr[0] != indxc[0]) { + ir = indxr[0]; + ic = indxc[0]; + temp = invOut(0,ir); + invOut(0,ir) = invOut(0,ic); + invOut(0,ic) = temp; + temp = invOut(1,ir); + invOut(1,ir) = invOut(1,ic); + invOut(1,ic) = temp; + temp = invOut(2,ir); + invOut(2,ir) = invOut(2,ic); + invOut(2,ic) = temp; + temp = invOut(3,ir); + invOut(3,ir) = invOut(3,ic); + invOut(3,ic) = temp; + } +end: + return invOut; + } + +private: + T m_aatData[SIZE][SIZE]; +}; +// some common vector classes (abbr. names) +typedef Mat Mat2f; +typedef Mat Mat3f; +typedef Mat Mat4f; + +typedef Mat Mat2d; +typedef Mat Mat3d; +typedef Mat Mat4d; + + +template +static std::ostream& operator<< (std::ostream& output,const Mat &mat) +{ + output << "( "; + for(unsigned int j =0; j< SIZE; ++j) + { + output << "( "; + for(unsigned int i =0; i< SIZE; ++i) + { + output << mat(j,i); + if (i<4-1) + output << " , "; + else + output << " )"; + } + if (j getRotationMatrix(float x_angle,float y_angle, float z_angle) +{ + Mat tempx,tempy,tempz; + float temp_x[4][4] = { { 1, 0, 0, 0}, + { 0, cos(x_angle), -sin(x_angle), 0}, + { 0, sin(x_angle), cos(x_angle), 0}, + { 0, 0, 0, 1} + }; + + float temp_y[4][4] = { { cos(y_angle), 0, -sin(y_angle), 0}, + { 0, 1, 0, 0}, + { sin(y_angle), 0, cos(y_angle), 0}, + { 0, 0, 0, 1} + }; + float temp_z[4][4] = { { cos(z_angle), -sin(z_angle), 0, 0}, + { sin(z_angle), cos(z_angle), 0, 0}, + { 0, 0, 1, 0}, + { 0, 0, 0, 1} + }; + + tempx=temp_x; + tempy=temp_y; + tempz=temp_z; + return tempz*tempy*tempx; +} +#endif diff --git a/lwrserv/program.cpp b/lwrserv/program.cpp new file mode 100644 index 0000000..90a2d5b --- /dev/null +++ b/lwrserv/program.cpp @@ -0,0 +1,520 @@ +#include "header.h" +#include "global.h" +#include "sgn.h" +#include +#include + +void printMat(Mat4f M) +{ +#if 0 + for (int i=0;i(i,j) << " "; + } + cout << "\n\r"; + } + cout << "\n\r\n\r"; +#else + cout << M << endl; +#endif +} + +float* abctomat(float a, float b, float c) +{ + Mat4f rx; + float ca = cos(c); + float sa = sin(c); + + rx(0,0) = 1; + rx(1,1) = ca; + rx(1,2) = -sa; + rx(2,1) = sa; + rx(2,2) = ca; + rx(3,3) = 1; + + Mat4f ry; + float cb = cos(b); + float sb = sin(b); + ry(0,0) = cb; + ry(0,2) = sb; + ry(1,1) = 1; + ry(2,0) = -sb; + ry(2,2) = cb; + ry(3,3) = 1; + + Mat4f rz; + float cc = cos(a); + float sc = sin(a); + rz(0,0) = cc; + rz(0,1) = -sc; + rz(1,0) = sc; + rz(1,1) = cc; + rz(2,2) = 1; + rz(3,3) = 1; + + Mat4f result; + Mat4f temp; + temp = rz * ry; + result = temp * rx; + +#ifdef __DEBUG__ + printMat(rx); + printMat(ry); + printMat(rz); + printMat(result); +#endif + + float *res = new float[12]; + //TODO simple converter for mat + for (int j=0;j<3;j++) + { + for (int k=0;k<4;k++) + { + res[j*4+k] = result(j,k); + } + } + return res; +} + +float* mattoabc(float M[12]) +{ + float norm; + float sa; + float ca; + float *abc = new float[3]; + + norm = sqrt((M[0]*M[0])+(M[4]*M[4])); + + if (norm>1e-5) + { + sa = M[4]/norm; + ca = M[0]/norm; + abc[0] = atan2(sa,ca); + } + else + { + sa = 0; + ca = 1; + abc[0] = 0; + } + abc[1] = atan2(-M[8],ca*M[0]+sa*M[4]); + abc[2] = atan2(sa*M[2]-ca*M[6],-sa*M[1]+ca*M[5]); + return abc; +} + +void doRalf() +{ + // Global definitions + float lowStiff[6] = { 0.01f, 0.01f, 0.01f, 0.01f, 0.01f, 0.01f}; + float highStiff[6] = {1000.0f, 1000.0f, 1000.0f, 10.0f, 10.0f, 10.0f}; + float slowDamp[6] = { 0.007f, 0.007f, 0.007f, 0.007f, 0.007f, 0.007f}; + float fastDamp[6] = { 0.7f, 0.7f, 0.7f, 0.7f, 0.7f, 0.7f}; + + float pos1[12] = { + 1.0f, 0.0f, 0.0f, 500.0f, + 0.0f, 1.0f, 0.0f, 0.0f, + 0.0f, 0.0f, 1.0f, 500.0f + }; + float pos2[12] = { + 1.0f, 0.0f, 0.0f , 500.0f, + 0.0f, 1.0f, 0.0f , 0.0f, + 0.0f, 0.0f ,1.0f , 500.0f + }; + + friRemote friInst; + double timeCounter=0; + friInst.doDataExchange(); + + float* cartPos; + while(true) + { + // Time + timeCounter+=friInst.getSampleTime(); + + friInst.doReceiveData(); + + // Get all variables from FRI + float* jntPos = friInst.getMsrMsrJntPosition(); + float* jacobian = friInst.getJacobian(); + float* ftTcp = friInst.getFTTcp(); + + if (timeCounter < 20) + cartPos = friInst.getMsrCartPosition(); + //friInst.doDataExchange(); + //friInst.doCartesianImpedanceControl(cartPos, highStiff, fastDamp, NULL, NULL, true); +#if 0 + if (timeCounter>10 && timeCounter<10) + { + friInst.doCartesianImpedanceControl(cartPos, highStiff, fastDamp, NULL, NULL, false); + } else + { + friInst.doCartesianImpedanceControl(cartPos, highStiff, fastDamp, NULL, NULL, false); + } +#endif + float npos[12]; + memcpy(npos,cartPos,12); + friInst.doCartesianImpedanceControl(npos, highStiff, fastDamp, NULL, NULL, false); + +#if 0 + if (timeCounter<20) + { + friInst.doCartesianImpedanceControl(Test, highStiff, fastDamp, NULL, NULL, true); + } else if (timeCounter<30) + { + friInst.doCartesianImpedanceControl(cartPos, highStiff, fastDamp, NULL, NULL, true); + } else if (timeCounter<40) + { + friInst.doCartesianImpedanceControl(cartPos , highStiff, fastDamp, NULL, NULL, true); + } else + { + friInst.doDataExchange(); + } +#endif + friInst.doSendData(); + } +} + +Mat4f vecToMat2(float vec[12]) +{ + Mat4f result; + for (int i=0; i<3; i++) + { + for (int j=0; j<4; j++) + { + result(i,j) = (float)vec[i*4+j]; + } + } + result(3,3)=(float)1; + return result; +} + +float* matToVec2(Mat4f mat) +{ + float* vec = new float[12]; + for (int j=0;j<3;j++) + { + for (int k=0;k<4;k++) + { + vec[j*4+k] = mat(j,k); + } + } + return vec; +} + +float* vectoquat(float vec[12]) +{ + float *quat = new float[4]; + float diag[3]; + int v; int w; int u; + diag[0] = vec[0]; + diag[1] = vec[5]; + diag[2] = vec[10]; + float u_ = *std::max_element(diag,diag+2); + + if (u_==diag[0]) + { + u = 1; + v = 2; + w = 3; + } else if (u_==diag[1]) + { + u = 2; + v = 3; + w = 1; + } else + { + u = 3; + v = 1; + w = 2; + } + float r = sqrt(1+vec[(u-1)*4+(u-1)] - vec[(v-1)*4+(v-1)] - vec[(w-1)*4+(w-1)]); + quat[0] = (vec[(w-1)*4+(v-1)] - vec[(v-1)*4+(w-1)]) / (2*r); + quat[u] = r/2; + quat[v] = (vec[(u-1)*4+(v-1)] + vec[(v-1)*4+(u-1)]) / (2*r); + quat[w] = (vec[(w-1)*4+(u-1)] + vec[(u-1)*4+(w-1)]) / (2*r); + + return quat; +} + +float* quattovec(float quat[4]) +{ + float *vec = new float[12]; + vec[0] = quat[0]*quat[0]+quat[1]*quat[1]-quat[2]*quat[2]-quat[3]*quat[3]; + vec[4] = 2*(quat[1]*quat[2]+quat[0]*quat[3]); + vec[8] = 2*(quat[1]*quat[3]-quat[0]*quat[2]); + + vec[1] = 2*(quat[1]*quat[2]-quat[0]*quat[3]); + vec[5] = quat[0]*quat[0]-quat[1]*quat[1]+quat[2]*quat[2]-quat[3]*quat[3]; + vec[9] = 2*(quat[2]*quat[3]+quat[0]*quat[1]); + + vec[2] = 2*(quat[1]*quat[3]+quat[0]*quat[2]); + vec[6] = 2*(quat[2]*quat[3]-quat[0]*quat[1]); + vec[10] = quat[0]*quat[0]-quat[1]*quat[1]-quat[2]*quat[2]+quat[3]*quat[3]; + + return vec; +} + + +void *threadFriDataExchange(void *arg) +{ + //doRalf(); + + friRemote friInst; + + while (1) + { + //####################################### + // Communication loop + + // get current joint positions + for ( int i= 0; i < LBR_MNJ; i++) + { + MSRMSRJNTPOS[i] = friInst.getMsrCmdJntPosition()[i] + friInst.getMsrCmdJntPositionOffset()[i]; + //MSRMSRJNTPOS[i] = friInst.getMsrMsrJntPosition()[i]; + } + + // get current jacobian + float* friJacobian = friInst.getJacobian(); + if ( friJacobian == NULL) + { + fprintf(stderr,"Failed: could not get jacobian\n"); + break; + } + for (int i = 0; i < FRI_CART_VEC*LBR_MNJ ; i++) + { + MSRMSRJACOBIAN[i] = friJacobian[i]; + } + + // get current messured carthesian position + float* friMsrCartPosition = friInst.getMsrCartPosition(); + if ( friMsrCartPosition == NULL) + { + fprintf(stderr,"Failed: could not get Messurement in Carthesian Position\n"); + break; + } + for ( int i =0; i < FRI_CART_FRM_DIM; i++) + { + MSRMSRCARTPOS[i] = friMsrCartPosition[i]; + } + + // get current force and torque + float* friFTTcp = friInst.getFTTcp(); + for ( int i = 0; i < FRI_CART_VEC; i++) + { + MSRMSRFTTCP[i] = friFTTcp[i]; + } + + //######################################################### + // PTP Joint Movement + if (__MSRCMDJNTPOS) + { + double maxVelJntLocal[LBR_MNJ]; + double maxAccJntLocal[LBR_MNJ]; + double delta[LBR_MNJ]; + double sampleTime = (double)0.005; + double deltaAbs[LBR_MNJ]; + double dMaxSpeed[LBR_MNJ]; + double lMaxSpeed[LBR_MNJ]; + double dGesamt[LBR_MNJ]; + + double currentInk[LBR_MNJ] = {0}; + double currentInkLast[LBR_MNJ] = {0}; + double currentDist [LBR_MNJ] = {0}; + double currentDistLast [LBR_MNJ] = {0}; + + // calculate delta positions of movement + for ( int i= 0; i < LBR_MNJ; i++) + { + delta[i] = MSRCMDJNTPOS[i]-(MSRMSRJNTPOS[i]*180/M_PI); + deltaAbs[i] = fabs(MSRCMDJNTPOS[i]-(MSRMSRJNTPOS[i]*180/M_PI)); + maxVelJntLocal[i] = maxVelJnt[i]*sampleTime*(VELOCITY/100.0); + maxAccJntLocal[i] = maxAccJnt[i]*sampleTime*(ACCELARATION/100.0); + } + + // calculate movement steps + for (int j=0; j deltaAbs[j]/(double)2.0) + { + dGesamt[j] = sqrt(deltaAbs[j]/maxAccJntLocal[j])*2.0; + } else + { + dGesamt[j] = dMaxSpeed[j]*2 + (deltaAbs[j]-lMaxSpeed[j]*2.0)/maxVelJntLocal[j]; + } + } + int maxSteps =ceil(*max_element(dGesamt,(dGesamt+LBR_MNJ-1))); + + if (maxSteps == 0 || maxSteps == 1) + goto end; + + for (int j=0; j maxSteps-dMaxSpeed[j]) + { + currentInk[j] = max(currentInkLast[j]-maxAccJntLocal[j],(double)0); + }else + { + currentInk[j] = currentInkLast[j]; + } + currentDist[j] = currentDistLast[j] + sgn(delta[j])*currentInk[j]; + currentInkLast[j] = currentInk[j]; + currentDistLast[j] = currentDist[j]; + MSRMSRJNTPOS[j]+= sgn(delta[j])*currentInk[j]*(1./180*M_PI); + } + friInst.doPositionControl(MSRMSRJNTPOS); + } +end: + __MSRCMDJNTPOS = false; + + } else if (__CARTMOVE) + { + //########################################################## + //Cartesian Movement + + const float Stiff[6] = {1000.0, 1000.0, 1000.0, 150.0, 150.0, 150.0}; + const float Damp[6] = {0.7, 0.7, 0.7, 0.7, 0.7, 0.7}; + float th = 0.000; + float deltaCart[12]; + float Pos[12]; + float dist; + float rot; + float sum; + float b; + float c; + float t_fact = 1; + float t_outerT = 0.007; + float t_innerT = 0.000; + float t_outerR = 0.007; + float t_innerR = 0.000; + + while (true) + { + for ( int i =0; i < FRI_CART_FRM_DIM; i++) + { + Pos[i] = friInst.getMsrCartPosition()[i]; + MSRMSRCARTPOS[i] = friInst.getMsrCartPosition()[i]; + deltaCart[i] = MSRCMDCARTPOS[i]-Pos[i]; + } + sum = 0; + for (int i=0; i<3 ; i++) + { + sum = sum + (deltaCart[(i*4)+3]*deltaCart[(i*4)+3]); + } + dist = sqrt(sum); + sum = 0; + for (int i=0;i<12;i++) + { + if (i != 3 && i != 7 && i != 11) + { + sum = sum + deltaCart[i]*deltaCart[i]; + } + } + rot = sqrt(sum); + b = t_fact*max((float)0.0,(float)min((float)1.0,(t_outerT-dist+(t_innerT-t_outerT))*(1/(t_innerT-t_outerT)))); + c = t_fact*max((float)0.0,(float)min((float)1.0,(float)(t_outerR-rot+(t_innerR-t_outerR))*(float)(1/(t_innerR-t_outerR)))); + + if (dist < th) + { + for (int i=0; i<3; i++) + { + deltaCart[(i*4)+3] = 0; + } + }else + { + for (int i=0; i<3; i++) + { + deltaCart[(i*4)+3] = (deltaCart[(i*4)+3] / dist)*b; + } + } + + // check offset to point + if (rot < th) + { + for (int i=0;i<12;i++) + { + if (i != 3 && i != 7 && i != 11) + { + deltaCart[i] = 0; + } + } + }else + { + for (int i=0;i<12;i++) + { + if (i != 3 && i != 7 && i != 11) + { + deltaCart[i] = (deltaCart[i] / rot)*c; + } + } + } + for (int i=0;i<12;i++) + { + Pos[i]+= deltaCart[i]*0.02; + } + friInst.doCartesianImpedanceControl(Pos, Stiff,Damp,NULL,NULL, true); + + } + } + //float Stiff[6] = {1000.0, 1000.0, 1000.0, 10.0, 10.0, 10.0}; + //float Damp[6] = {0.7, 0.7, 0.7, 0.7, 0.7, 0.7}; + //float Test[12] = {MSRMSRCARTPOS[0],MSRMSRCARTPOS[1],MSRMSRCARTPOS[2],MSRMSRCARTPOS[3],MSRMSRCARTPOS[4],MSRMSRCARTPOS[5],MSRMSRCARTPOS[6],MSRMSRCARTPOS[7],MSRMSRCARTPOS[8],MSRMSRCARTPOS[9],MSRMSRCARTPOS[10],MSRMSRCARTPOS[11]}; + //friInst.doCartesianImpedanceControl(Test,Stiff, Damp, NULL, NULL, true); + + friInst.doPositionControl(MSRMSRJNTPOS); + + //i = 0; + //} + //friInst.doCartesianImpedanceControl(Test,Stiff, Damp, NULL, NULL, true); + } + return NULL; +} + +int main() +{ + int err = 0; + //Setting pthread for FRI interface + pthread_t fri_t; + //Start fri_thread + err = pthread_create(&fri_t,NULL,&threadFriDataExchange,NULL); + if (err > 0 ) + { + cerr << "Failed: could not create thread\n" << endl; + return err; + } + + + //Start client handling + SvrHandling *svr = new SvrHandling(); + if (svr == NULL) + { + cerr << "Failed: could not create server \n" << endl; + return -ENOMEM; + } + svr->run(SVR_DEFAULT_PORT); + + return 0; +} diff --git a/lwrserv/pthread/pthread.h b/lwrserv/pthread/pthread.h new file mode 100755 index 0000000..436ea8b --- /dev/null +++ b/lwrserv/pthread/pthread.h @@ -0,0 +1,1368 @@ +/* This is an implementation of the threads API of POSIX 1003.1-2001. + * + * -------------------------------------------------------------------------- + * + * Pthreads-win32 - POSIX Threads Library for Win32 + * Copyright(C) 1998 John E. Bossom + * Copyright(C) 1999,2005 Pthreads-win32 contributors + * + * Contact Email: rpj@callisto.canberra.edu.au + * + * The current list of contributors is contained + * in the file CONTRIBUTORS included with the source + * code distribution. The list can also be seen at the + * following World Wide Web location: + * http://sources.redhat.com/pthreads-win32/contributors.html + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2 of the License, or (at your option) any later version. + * + * This library 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 + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library in the file COPYING.LIB; + * if not, write to the Free Software Foundation, Inc., + * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA + */ + +#if !defined( PTHREAD_H ) +#define PTHREAD_H + +/* + * See the README file for an explanation of the pthreads-win32 version + * numbering scheme and how the DLL is named etc. + */ +#define PTW32_VERSION 2,9,1,0 +#define PTW32_VERSION_STRING "2, 9, 1, 0\0" + +/* There are three implementations of cancel cleanup. + * Note that pthread.h is included in both application + * compilation units and also internally for the library. + * The code here and within the library aims to work + * for all reasonable combinations of environments. + * + * The three implementations are: + * + * WIN32 SEH + * C + * C++ + * + * Please note that exiting a push/pop block via + * "return", "exit", "break", or "continue" will + * lead to different behaviour amongst applications + * depending upon whether the library was built + * using SEH, C++, or C. For example, a library built + * with SEH will call the cleanup routine, while both + * C++ and C built versions will not. + */ + +/* + * Define defaults for cleanup code. + * Note: Unless the build explicitly defines one of the following, then + * we default to standard C style cleanup. This style uses setjmp/longjmp + * in the cancelation and thread exit implementations and therefore won't + * do stack unwinding if linked to applications that have it (e.g. + * C++ apps). This is currently consistent with most/all commercial Unix + * POSIX threads implementations. + */ +#if !defined( __CLEANUP_SEH ) && !defined( __CLEANUP_CXX ) && !defined( __CLEANUP_C ) +# define __CLEANUP_C +#endif + +#if defined( __CLEANUP_SEH ) && ( !defined( _MSC_VER ) && !defined(PTW32_RC_MSC)) +#error ERROR [__FILE__, line __LINE__]: SEH is not supported for this compiler. +#endif + +/* + * Stop here if we are being included by the resource compiler. + */ +#if !defined(RC_INVOKED) + +#undef PTW32_LEVEL + +#if defined(_POSIX_SOURCE) +#define PTW32_LEVEL 0 +/* Early POSIX */ +#endif + +#if defined(_POSIX_C_SOURCE) && _POSIX_C_SOURCE >= 199309 +#undef PTW32_LEVEL +#define PTW32_LEVEL 1 +/* Include 1b, 1c and 1d */ +#endif + +#if defined(INCLUDE_NP) +#undef PTW32_LEVEL +#define PTW32_LEVEL 2 +/* Include Non-Portable extensions */ +#endif + +#define PTW32_LEVEL_MAX 3 + +#if ( defined(_POSIX_C_SOURCE) && _POSIX_C_SOURCE >= 200112 ) || !defined(PTW32_LEVEL) +#define PTW32_LEVEL PTW32_LEVEL_MAX +/* Include everything */ +#endif + +#if defined(_UWIN) +# define HAVE_STRUCT_TIMESPEC 1 +# define HAVE_SIGNAL_H 1 +# undef HAVE_PTW32_CONFIG_H +# pragma comment(lib, "pthread") +#endif + +/* + * ------------------------------------------------------------- + * + * + * Module: pthread.h + * + * Purpose: + * Provides an implementation of PThreads based upon the + * standard: + * + * POSIX 1003.1-2001 + * and + * The Single Unix Specification version 3 + * + * (these two are equivalent) + * + * in order to enhance code portability between Windows, + * various commercial Unix implementations, and Linux. + * + * See the ANNOUNCE file for a full list of conforming + * routines and defined constants, and a list of missing + * routines and constants not defined in this implementation. + * + * Authors: + * There have been many contributors to this library. + * The initial implementation was contributed by + * John Bossom, and several others have provided major + * sections or revisions of parts of the implementation. + * Often significant effort has been contributed to + * find and fix important bugs and other problems to + * improve the reliability of the library, which sometimes + * is not reflected in the amount of code which changed as + * result. + * As much as possible, the contributors are acknowledged + * in the ChangeLog file in the source code distribution + * where their changes are noted in detail. + * + * Contributors are listed in the CONTRIBUTORS file. + * + * As usual, all bouquets go to the contributors, and all + * brickbats go to the project maintainer. + * + * Maintainer: + * The code base for this project is coordinated and + * eventually pre-tested, packaged, and made available by + * + * Ross Johnson + * + * QA Testers: + * Ultimately, the library is tested in the real world by + * a host of competent and demanding scientists and + * engineers who report bugs and/or provide solutions + * which are then fixed or incorporated into subsequent + * versions of the library. Each time a bug is fixed, a + * test case is written to prove the fix and ensure + * that later changes to the code don't reintroduce the + * same error. The number of test cases is slowly growing + * and therefore so is the code reliability. + * + * Compliance: + * See the file ANNOUNCE for the list of implemented + * and not-implemented routines and defined options. + * Of course, these are all defined is this file as well. + * + * Web site: + * The source code and other information about this library + * are available from + * + * http://sources.redhat.com/pthreads-win32/ + * + * ------------------------------------------------------------- + */ + +/* Try to avoid including windows.h */ +#if (defined(__MINGW64__) || defined(__MINGW32__)) && defined(__cplusplus) +#define PTW32_INCLUDE_WINDOWS_H +#endif + +#if defined(PTW32_INCLUDE_WINDOWS_H) +#include +#endif + +#if defined(_MSC_VER) && _MSC_VER < 1300 || defined(__DMC__) +/* + * VC++6.0 or early compiler's header has no DWORD_PTR type. + */ +typedef unsigned long DWORD_PTR; +typedef unsigned long ULONG_PTR; +#endif +/* + * ----------------- + * autoconf switches + * ----------------- + */ + +#if defined(HAVE_PTW32_CONFIG_H) +#include "config.h" +#endif /* HAVE_PTW32_CONFIG_H */ + +#if !defined(NEED_FTIME) +#include +#else /* NEED_FTIME */ +/* use native WIN32 time API */ +#endif /* NEED_FTIME */ + +#if defined(HAVE_SIGNAL_H) +#include +#endif /* HAVE_SIGNAL_H */ + +#include + +/* + * Boolean values to make us independent of system includes. + */ +enum { + PTW32_FALSE = 0, + PTW32_TRUE = (! PTW32_FALSE) +}; + +/* + * This is a duplicate of what is in the autoconf config.h, + * which is only used when building the pthread-win32 libraries. + */ + +#if !defined(PTW32_CONFIG_H) +# if defined(WINCE) +# define NEED_ERRNO +# define NEED_SEM +# endif +# if defined(__MINGW64__) +# define HAVE_STRUCT_TIMESPEC +# define HAVE_MODE_T +# elif defined(_UWIN) || defined(__MINGW32__) +# define HAVE_MODE_T +# endif +#endif + +/* + * + */ + +#if PTW32_LEVEL >= PTW32_LEVEL_MAX +#if defined(NEED_ERRNO) +#include "need_errno.h" +#else +#include +#endif +#endif /* PTW32_LEVEL >= PTW32_LEVEL_MAX */ + +/* + * Several systems don't define some error numbers. + */ +#if !defined(ENOTSUP) +# define ENOTSUP 48 /* This is the value in Solaris. */ +#endif + +#if !defined(ETIMEDOUT) +# define ETIMEDOUT 10060 /* Same as WSAETIMEDOUT */ +#endif + +#if !defined(ENOSYS) +# define ENOSYS 140 /* Semi-arbitrary value */ +#endif + +#if !defined(EDEADLK) +# if defined(EDEADLOCK) +# define EDEADLK EDEADLOCK +# else +# define EDEADLK 36 /* This is the value in MSVC. */ +# endif +#endif + +/* POSIX 2008 - related to robust mutexes */ +#if !defined(EOWNERDEAD) +# define EOWNERDEAD 43 +#endif +#if !defined(ENOTRECOVERABLE) +# define ENOTRECOVERABLE 44 +#endif + +#include + +/* + * To avoid including windows.h we define only those things that we + * actually need from it. + */ +#if !defined(PTW32_INCLUDE_WINDOWS_H) +#if !defined(HANDLE) +# define PTW32__HANDLE_DEF +# define HANDLE void * +#endif +#if !defined(DWORD) +# define PTW32__DWORD_DEF +# define DWORD unsigned long +#endif +#endif + +#if !defined(HAVE_STRUCT_TIMESPEC) +#define HAVE_STRUCT_TIMESPEC +#if !defined(_TIMESPEC_DEFINED) +#define _TIMESPEC_DEFINED +struct timespec { + time_t tv_sec; + long tv_nsec; +}; +#endif /* _TIMESPEC_DEFINED */ +#endif /* HAVE_STRUCT_TIMESPEC */ + +#if !defined(SIG_BLOCK) +#define SIG_BLOCK 0 +#endif /* SIG_BLOCK */ + +#if !defined(SIG_UNBLOCK) +#define SIG_UNBLOCK 1 +#endif /* SIG_UNBLOCK */ + +#if !defined(SIG_SETMASK) +#define SIG_SETMASK 2 +#endif /* SIG_SETMASK */ + +#if defined(__cplusplus) +extern "C" +{ +#endif /* __cplusplus */ + +/* + * ------------------------------------------------------------- + * + * POSIX 1003.1-2001 Options + * ========================= + * + * Options are normally set in , which is not provided + * with pthreads-win32. + * + * For conformance with the Single Unix Specification (version 3), all of the + * options below are defined, and have a value of either -1 (not supported) + * or 200112L (supported). + * + * These options can neither be left undefined nor have a value of 0, because + * either indicates that sysconf(), which is not implemented, may be used at + * runtime to check the status of the option. + * + * _POSIX_THREADS (== 200112L) + * If == 200112L, you can use threads + * + * _POSIX_THREAD_ATTR_STACKSIZE (== 200112L) + * If == 200112L, you can control the size of a thread's + * stack + * pthread_attr_getstacksize + * pthread_attr_setstacksize + * + * _POSIX_THREAD_ATTR_STACKADDR (== -1) + * If == 200112L, you can allocate and control a thread's + * stack. If not supported, the following functions + * will return ENOSYS, indicating they are not + * supported: + * pthread_attr_getstackaddr + * pthread_attr_setstackaddr + * + * _POSIX_THREAD_PRIORITY_SCHEDULING (== -1) + * If == 200112L, you can use realtime scheduling. + * This option indicates that the behaviour of some + * implemented functions conforms to the additional TPS + * requirements in the standard. E.g. rwlocks favour + * writers over readers when threads have equal priority. + * + * _POSIX_THREAD_PRIO_INHERIT (== -1) + * If == 200112L, you can create priority inheritance + * mutexes. + * pthread_mutexattr_getprotocol + + * pthread_mutexattr_setprotocol + + * + * _POSIX_THREAD_PRIO_PROTECT (== -1) + * If == 200112L, you can create priority ceiling mutexes + * Indicates the availability of: + * pthread_mutex_getprioceiling + * pthread_mutex_setprioceiling + * pthread_mutexattr_getprioceiling + * pthread_mutexattr_getprotocol + + * pthread_mutexattr_setprioceiling + * pthread_mutexattr_setprotocol + + * + * _POSIX_THREAD_PROCESS_SHARED (== -1) + * If set, you can create mutexes and condition + * variables that can be shared with another + * process.If set, indicates the availability + * of: + * pthread_mutexattr_getpshared + * pthread_mutexattr_setpshared + * pthread_condattr_getpshared + * pthread_condattr_setpshared + * + * _POSIX_THREAD_SAFE_FUNCTIONS (== 200112L) + * If == 200112L you can use the special *_r library + * functions that provide thread-safe behaviour + * + * _POSIX_READER_WRITER_LOCKS (== 200112L) + * If == 200112L, you can use read/write locks + * + * _POSIX_SPIN_LOCKS (== 200112L) + * If == 200112L, you can use spin locks + * + * _POSIX_BARRIERS (== 200112L) + * If == 200112L, you can use barriers + * + * + These functions provide both 'inherit' and/or + * 'protect' protocol, based upon these macro + * settings. + * + * ------------------------------------------------------------- + */ + +/* + * POSIX Options + */ +#undef _POSIX_THREADS +#define _POSIX_THREADS 200809L + +#undef _POSIX_READER_WRITER_LOCKS +#define _POSIX_READER_WRITER_LOCKS 200809L + +#undef _POSIX_SPIN_LOCKS +#define _POSIX_SPIN_LOCKS 200809L + +#undef _POSIX_BARRIERS +#define _POSIX_BARRIERS 200809L + +#undef _POSIX_THREAD_SAFE_FUNCTIONS +#define _POSIX_THREAD_SAFE_FUNCTIONS 200809L + +#undef _POSIX_THREAD_ATTR_STACKSIZE +#define _POSIX_THREAD_ATTR_STACKSIZE 200809L + +/* + * The following options are not supported + */ +#undef _POSIX_THREAD_ATTR_STACKADDR +#define _POSIX_THREAD_ATTR_STACKADDR -1 + +#undef _POSIX_THREAD_PRIO_INHERIT +#define _POSIX_THREAD_PRIO_INHERIT -1 + +#undef _POSIX_THREAD_PRIO_PROTECT +#define _POSIX_THREAD_PRIO_PROTECT -1 + +/* TPS is not fully supported. */ +#undef _POSIX_THREAD_PRIORITY_SCHEDULING +#define _POSIX_THREAD_PRIORITY_SCHEDULING -1 + +#undef _POSIX_THREAD_PROCESS_SHARED +#define _POSIX_THREAD_PROCESS_SHARED -1 + + +/* + * POSIX 1003.1-2001 Limits + * =========================== + * + * These limits are normally set in , which is not provided with + * pthreads-win32. + * + * PTHREAD_DESTRUCTOR_ITERATIONS + * Maximum number of attempts to destroy + * a thread's thread-specific data on + * termination (must be at least 4) + * + * PTHREAD_KEYS_MAX + * Maximum number of thread-specific data keys + * available per process (must be at least 128) + * + * PTHREAD_STACK_MIN + * Minimum supported stack size for a thread + * + * PTHREAD_THREADS_MAX + * Maximum number of threads supported per + * process (must be at least 64). + * + * SEM_NSEMS_MAX + * The maximum number of semaphores a process can have. + * (must be at least 256) + * + * SEM_VALUE_MAX + * The maximum value a semaphore can have. + * (must be at least 32767) + * + */ +#undef _POSIX_THREAD_DESTRUCTOR_ITERATIONS +#define _POSIX_THREAD_DESTRUCTOR_ITERATIONS 4 + +#undef PTHREAD_DESTRUCTOR_ITERATIONS +#define PTHREAD_DESTRUCTOR_ITERATIONS _POSIX_THREAD_DESTRUCTOR_ITERATIONS + +#undef _POSIX_THREAD_KEYS_MAX +#define _POSIX_THREAD_KEYS_MAX 128 + +#undef PTHREAD_KEYS_MAX +#define PTHREAD_KEYS_MAX _POSIX_THREAD_KEYS_MAX + +#undef PTHREAD_STACK_MIN +#define PTHREAD_STACK_MIN 0 + +#undef _POSIX_THREAD_THREADS_MAX +#define _POSIX_THREAD_THREADS_MAX 64 + + /* Arbitrary value */ +#undef PTHREAD_THREADS_MAX +#define PTHREAD_THREADS_MAX 2019 + +#undef _POSIX_SEM_NSEMS_MAX +#define _POSIX_SEM_NSEMS_MAX 256 + + /* Arbitrary value */ +#undef SEM_NSEMS_MAX +#define SEM_NSEMS_MAX 1024 + +#undef _POSIX_SEM_VALUE_MAX +#define _POSIX_SEM_VALUE_MAX 32767 + +#undef SEM_VALUE_MAX +#define SEM_VALUE_MAX INT_MAX + + +#if defined(__GNUC__) && !defined(__declspec) +# error Please upgrade your GNU compiler to one that supports __declspec. +#endif + +/* + * When building the library, you should define PTW32_BUILD so that + * the variables/functions are exported correctly. When using the library, + * do NOT define PTW32_BUILD, and then the variables/functions will + * be imported correctly. + */ +#if !defined(PTW32_STATIC_LIB) +# if defined(PTW32_BUILD) +# define PTW32_DLLPORT __declspec (dllexport) +# else +# define PTW32_DLLPORT __declspec (dllimport) +# endif +#else +# define PTW32_DLLPORT +#endif + +/* + * The Open Watcom C/C++ compiler uses a non-standard calling convention + * that passes function args in registers unless __cdecl is explicitly specified + * in exposed function prototypes. + * + * We force all calls to cdecl even though this could slow Watcom code down + * slightly. If you know that the Watcom compiler will be used to build both + * the DLL and application, then you can probably define this as a null string. + * Remember that pthread.h (this file) is used for both the DLL and application builds. + */ +#define PTW32_CDECL __cdecl + +#if defined(_UWIN) && PTW32_LEVEL >= PTW32_LEVEL_MAX +# include +#else +/* + * Generic handle type - intended to extend uniqueness beyond + * that available with a simple pointer. It should scale for either + * IA-32 or IA-64. + */ +typedef struct { + void * p; /* Pointer to actual object */ + unsigned int x; /* Extra information - reuse count etc */ +} ptw32_handle_t; + +typedef ptw32_handle_t pthread_t; +typedef struct pthread_attr_t_ * pthread_attr_t; +typedef struct pthread_once_t_ pthread_once_t; +typedef struct pthread_key_t_ * pthread_key_t; +typedef struct pthread_mutex_t_ * pthread_mutex_t; +typedef struct pthread_mutexattr_t_ * pthread_mutexattr_t; +typedef struct pthread_cond_t_ * pthread_cond_t; +typedef struct pthread_condattr_t_ * pthread_condattr_t; +#endif +typedef struct pthread_rwlock_t_ * pthread_rwlock_t; +typedef struct pthread_rwlockattr_t_ * pthread_rwlockattr_t; +typedef struct pthread_spinlock_t_ * pthread_spinlock_t; +typedef struct pthread_barrier_t_ * pthread_barrier_t; +typedef struct pthread_barrierattr_t_ * pthread_barrierattr_t; + +/* + * ==================== + * ==================== + * POSIX Threads + * ==================== + * ==================== + */ + +enum { +/* + * pthread_attr_{get,set}detachstate + */ + PTHREAD_CREATE_JOINABLE = 0, /* Default */ + PTHREAD_CREATE_DETACHED = 1, + +/* + * pthread_attr_{get,set}inheritsched + */ + PTHREAD_INHERIT_SCHED = 0, + PTHREAD_EXPLICIT_SCHED = 1, /* Default */ + +/* + * pthread_{get,set}scope + */ + PTHREAD_SCOPE_PROCESS = 0, + PTHREAD_SCOPE_SYSTEM = 1, /* Default */ + +/* + * pthread_setcancelstate paramters + */ + PTHREAD_CANCEL_ENABLE = 0, /* Default */ + PTHREAD_CANCEL_DISABLE = 1, + +/* + * pthread_setcanceltype parameters + */ + PTHREAD_CANCEL_ASYNCHRONOUS = 0, + PTHREAD_CANCEL_DEFERRED = 1, /* Default */ + +/* + * pthread_mutexattr_{get,set}pshared + * pthread_condattr_{get,set}pshared + */ + PTHREAD_PROCESS_PRIVATE = 0, + PTHREAD_PROCESS_SHARED = 1, + +/* + * pthread_mutexattr_{get,set}robust + */ + PTHREAD_MUTEX_STALLED = 0, /* Default */ + PTHREAD_MUTEX_ROBUST = 1, + +/* + * pthread_barrier_wait + */ + PTHREAD_BARRIER_SERIAL_THREAD = -1 +}; + +/* + * ==================== + * ==================== + * Cancelation + * ==================== + * ==================== + */ +#define PTHREAD_CANCELED ((void *)(size_t) -1) + + +/* + * ==================== + * ==================== + * Once Key + * ==================== + * ==================== + */ +#define PTHREAD_ONCE_INIT { PTW32_FALSE, 0, 0, 0} + +struct pthread_once_t_ +{ + int done; /* indicates if user function has been executed */ + void * lock; + int reserved1; + int reserved2; +}; + + +/* + * ==================== + * ==================== + * Object initialisers + * ==================== + * ==================== + */ +#define PTHREAD_MUTEX_INITIALIZER ((pthread_mutex_t)(size_t) -1) +#define PTHREAD_RECURSIVE_MUTEX_INITIALIZER ((pthread_mutex_t)(size_t) -2) +#define PTHREAD_ERRORCHECK_MUTEX_INITIALIZER ((pthread_mutex_t)(size_t) -3) + +/* + * Compatibility with LinuxThreads + */ +#define PTHREAD_RECURSIVE_MUTEX_INITIALIZER_NP PTHREAD_RECURSIVE_MUTEX_INITIALIZER +#define PTHREAD_ERRORCHECK_MUTEX_INITIALIZER_NP PTHREAD_ERRORCHECK_MUTEX_INITIALIZER + +#define PTHREAD_COND_INITIALIZER ((pthread_cond_t)(size_t) -1) + +#define PTHREAD_RWLOCK_INITIALIZER ((pthread_rwlock_t)(size_t) -1) + +#define PTHREAD_SPINLOCK_INITIALIZER ((pthread_spinlock_t)(size_t) -1) + + +/* + * Mutex types. + */ +enum +{ + /* Compatibility with LinuxThreads */ + PTHREAD_MUTEX_FAST_NP, + PTHREAD_MUTEX_RECURSIVE_NP, + PTHREAD_MUTEX_ERRORCHECK_NP, + PTHREAD_MUTEX_TIMED_NP = PTHREAD_MUTEX_FAST_NP, + PTHREAD_MUTEX_ADAPTIVE_NP = PTHREAD_MUTEX_FAST_NP, + /* For compatibility with POSIX */ + PTHREAD_MUTEX_NORMAL = PTHREAD_MUTEX_FAST_NP, + PTHREAD_MUTEX_RECURSIVE = PTHREAD_MUTEX_RECURSIVE_NP, + PTHREAD_MUTEX_ERRORCHECK = PTHREAD_MUTEX_ERRORCHECK_NP, + PTHREAD_MUTEX_DEFAULT = PTHREAD_MUTEX_NORMAL +}; + + +typedef struct ptw32_cleanup_t ptw32_cleanup_t; + +#if defined(_MSC_VER) +/* Disable MSVC 'anachronism used' warning */ +#pragma warning( disable : 4229 ) +#endif + +typedef void (* PTW32_CDECL ptw32_cleanup_callback_t)(void *); + +#if defined(_MSC_VER) +#pragma warning( default : 4229 ) +#endif + +struct ptw32_cleanup_t +{ + ptw32_cleanup_callback_t routine; + void *arg; + struct ptw32_cleanup_t *prev; +}; + +#if defined(__CLEANUP_SEH) + /* + * WIN32 SEH version of cancel cleanup. + */ + +#define pthread_cleanup_push( _rout, _arg ) \ + { \ + ptw32_cleanup_t _cleanup; \ + \ + _cleanup.routine = (ptw32_cleanup_callback_t)(_rout); \ + _cleanup.arg = (_arg); \ + __try \ + { \ + +#define pthread_cleanup_pop( _execute ) \ + } \ + __finally \ + { \ + if( _execute || AbnormalTermination()) \ + { \ + (*(_cleanup.routine))( _cleanup.arg ); \ + } \ + } \ + } + +#else /* __CLEANUP_SEH */ + +#if defined(__CLEANUP_C) + + /* + * C implementation of PThreads cancel cleanup + */ + +#define pthread_cleanup_push( _rout, _arg ) \ + { \ + ptw32_cleanup_t _cleanup; \ + \ + ptw32_push_cleanup( &_cleanup, (ptw32_cleanup_callback_t) (_rout), (_arg) ); \ + +#define pthread_cleanup_pop( _execute ) \ + (void) ptw32_pop_cleanup( _execute ); \ + } + +#else /* __CLEANUP_C */ + +#if defined(__CLEANUP_CXX) + + /* + * C++ version of cancel cleanup. + * - John E. Bossom. + */ + + class PThreadCleanup { + /* + * PThreadCleanup + * + * Purpose + * This class is a C++ helper class that is + * used to implement pthread_cleanup_push/ + * pthread_cleanup_pop. + * The destructor of this class automatically + * pops the pushed cleanup routine regardless + * of how the code exits the scope + * (i.e. such as by an exception) + */ + ptw32_cleanup_callback_t cleanUpRout; + void * obj; + int executeIt; + + public: + PThreadCleanup() : + cleanUpRout( 0 ), + obj( 0 ), + executeIt( 0 ) + /* + * No cleanup performed + */ + { + } + + PThreadCleanup( + ptw32_cleanup_callback_t routine, + void * arg ) : + cleanUpRout( routine ), + obj( arg ), + executeIt( 1 ) + /* + * Registers a cleanup routine for 'arg' + */ + { + } + + ~PThreadCleanup() + { + if ( executeIt && ((void *) cleanUpRout != (void *) 0) ) + { + (void) (*cleanUpRout)( obj ); + } + } + + void execute( int exec ) + { + executeIt = exec; + } + }; + + /* + * C++ implementation of PThreads cancel cleanup; + * This implementation takes advantage of a helper + * class who's destructor automatically calls the + * cleanup routine if we exit our scope weirdly + */ +#define pthread_cleanup_push( _rout, _arg ) \ + { \ + PThreadCleanup cleanup((ptw32_cleanup_callback_t)(_rout), \ + (void *) (_arg) ); + +#define pthread_cleanup_pop( _execute ) \ + cleanup.execute( _execute ); \ + } + +#else + +#error ERROR [__FILE__, line __LINE__]: Cleanup type undefined. + +#endif /* __CLEANUP_CXX */ + +#endif /* __CLEANUP_C */ + +#endif /* __CLEANUP_SEH */ + +/* + * =============== + * =============== + * Methods + * =============== + * =============== + */ + +/* + * PThread Attribute Functions + */ +PTW32_DLLPORT int PTW32_CDECL pthread_attr_init (pthread_attr_t * attr); + +PTW32_DLLPORT int PTW32_CDECL pthread_attr_destroy (pthread_attr_t * attr); + +PTW32_DLLPORT int PTW32_CDECL pthread_attr_getdetachstate (const pthread_attr_t * attr, + int *detachstate); + +PTW32_DLLPORT int PTW32_CDECL pthread_attr_getstackaddr (const pthread_attr_t * attr, + void **stackaddr); + +PTW32_DLLPORT int PTW32_CDECL pthread_attr_getstacksize (const pthread_attr_t * attr, + size_t * stacksize); + +PTW32_DLLPORT int PTW32_CDECL pthread_attr_setdetachstate (pthread_attr_t * attr, + int detachstate); + +PTW32_DLLPORT int PTW32_CDECL pthread_attr_setstackaddr (pthread_attr_t * attr, + void *stackaddr); + +PTW32_DLLPORT int PTW32_CDECL pthread_attr_setstacksize (pthread_attr_t * attr, + size_t stacksize); + +PTW32_DLLPORT int PTW32_CDECL pthread_attr_getschedparam (const pthread_attr_t *attr, + struct sched_param *param); + +PTW32_DLLPORT int PTW32_CDECL pthread_attr_setschedparam (pthread_attr_t *attr, + const struct sched_param *param); + +PTW32_DLLPORT int PTW32_CDECL pthread_attr_setschedpolicy (pthread_attr_t *, + int); + +PTW32_DLLPORT int PTW32_CDECL pthread_attr_getschedpolicy (const pthread_attr_t *, + int *); + +PTW32_DLLPORT int PTW32_CDECL pthread_attr_setinheritsched(pthread_attr_t * attr, + int inheritsched); + +PTW32_DLLPORT int PTW32_CDECL pthread_attr_getinheritsched(const pthread_attr_t * attr, + int * inheritsched); + +PTW32_DLLPORT int PTW32_CDECL pthread_attr_setscope (pthread_attr_t *, + int); + +PTW32_DLLPORT int PTW32_CDECL pthread_attr_getscope (const pthread_attr_t *, + int *); + +/* + * PThread Functions + */ +PTW32_DLLPORT int PTW32_CDECL pthread_create (pthread_t * tid, + const pthread_attr_t * attr, + void *(PTW32_CDECL *start) (void *), + void *arg); + +PTW32_DLLPORT int PTW32_CDECL pthread_detach (pthread_t tid); + +PTW32_DLLPORT int PTW32_CDECL pthread_equal (pthread_t t1, + pthread_t t2); + +PTW32_DLLPORT void PTW32_CDECL pthread_exit (void *value_ptr); + +PTW32_DLLPORT int PTW32_CDECL pthread_join (pthread_t thread, + void **value_ptr); + +PTW32_DLLPORT pthread_t PTW32_CDECL pthread_self (void); + +PTW32_DLLPORT int PTW32_CDECL pthread_cancel (pthread_t thread); + +PTW32_DLLPORT int PTW32_CDECL pthread_setcancelstate (int state, + int *oldstate); + +PTW32_DLLPORT int PTW32_CDECL pthread_setcanceltype (int type, + int *oldtype); + +PTW32_DLLPORT void PTW32_CDECL pthread_testcancel (void); + +PTW32_DLLPORT int PTW32_CDECL pthread_once (pthread_once_t * once_control, + void (PTW32_CDECL *init_routine) (void)); + +#if PTW32_LEVEL >= PTW32_LEVEL_MAX +PTW32_DLLPORT ptw32_cleanup_t * PTW32_CDECL ptw32_pop_cleanup (int execute); + +PTW32_DLLPORT void PTW32_CDECL ptw32_push_cleanup (ptw32_cleanup_t * cleanup, + ptw32_cleanup_callback_t routine, + void *arg); +#endif /* PTW32_LEVEL >= PTW32_LEVEL_MAX */ + +/* + * Thread Specific Data Functions + */ +PTW32_DLLPORT int PTW32_CDECL pthread_key_create (pthread_key_t * key, + void (PTW32_CDECL *destructor) (void *)); + +PTW32_DLLPORT int PTW32_CDECL pthread_key_delete (pthread_key_t key); + +PTW32_DLLPORT int PTW32_CDECL pthread_setspecific (pthread_key_t key, + const void *value); + +PTW32_DLLPORT void * PTW32_CDECL pthread_getspecific (pthread_key_t key); + + +/* + * Mutex Attribute Functions + */ +PTW32_DLLPORT int PTW32_CDECL pthread_mutexattr_init (pthread_mutexattr_t * attr); + +PTW32_DLLPORT int PTW32_CDECL pthread_mutexattr_destroy (pthread_mutexattr_t * attr); + +PTW32_DLLPORT int PTW32_CDECL pthread_mutexattr_getpshared (const pthread_mutexattr_t + * attr, + int *pshared); + +PTW32_DLLPORT int PTW32_CDECL pthread_mutexattr_setpshared (pthread_mutexattr_t * attr, + int pshared); + +PTW32_DLLPORT int PTW32_CDECL pthread_mutexattr_settype (pthread_mutexattr_t * attr, int kind); +PTW32_DLLPORT int PTW32_CDECL pthread_mutexattr_gettype (const pthread_mutexattr_t * attr, int *kind); + +PTW32_DLLPORT int PTW32_CDECL pthread_mutexattr_setrobust( + pthread_mutexattr_t *attr, + int robust); +PTW32_DLLPORT int PTW32_CDECL pthread_mutexattr_getrobust( + const pthread_mutexattr_t * attr, + int * robust); + +/* + * Barrier Attribute Functions + */ +PTW32_DLLPORT int PTW32_CDECL pthread_barrierattr_init (pthread_barrierattr_t * attr); + +PTW32_DLLPORT int PTW32_CDECL pthread_barrierattr_destroy (pthread_barrierattr_t * attr); + +PTW32_DLLPORT int PTW32_CDECL pthread_barrierattr_getpshared (const pthread_barrierattr_t + * attr, + int *pshared); + +PTW32_DLLPORT int PTW32_CDECL pthread_barrierattr_setpshared (pthread_barrierattr_t * attr, + int pshared); + +/* + * Mutex Functions + */ +PTW32_DLLPORT int PTW32_CDECL pthread_mutex_init (pthread_mutex_t * mutex, + const pthread_mutexattr_t * attr); + +PTW32_DLLPORT int PTW32_CDECL pthread_mutex_destroy (pthread_mutex_t * mutex); + +PTW32_DLLPORT int PTW32_CDECL pthread_mutex_lock (pthread_mutex_t * mutex); + +PTW32_DLLPORT int PTW32_CDECL pthread_mutex_timedlock(pthread_mutex_t * mutex, + const struct timespec *abstime); + +PTW32_DLLPORT int PTW32_CDECL pthread_mutex_trylock (pthread_mutex_t * mutex); + +PTW32_DLLPORT int PTW32_CDECL pthread_mutex_unlock (pthread_mutex_t * mutex); + +PTW32_DLLPORT int PTW32_CDECL pthread_mutex_consistent (pthread_mutex_t * mutex); + +/* + * Spinlock Functions + */ +PTW32_DLLPORT int PTW32_CDECL pthread_spin_init (pthread_spinlock_t * lock, int pshared); + +PTW32_DLLPORT int PTW32_CDECL pthread_spin_destroy (pthread_spinlock_t * lock); + +PTW32_DLLPORT int PTW32_CDECL pthread_spin_lock (pthread_spinlock_t * lock); + +PTW32_DLLPORT int PTW32_CDECL pthread_spin_trylock (pthread_spinlock_t * lock); + +PTW32_DLLPORT int PTW32_CDECL pthread_spin_unlock (pthread_spinlock_t * lock); + +/* + * Barrier Functions + */ +PTW32_DLLPORT int PTW32_CDECL pthread_barrier_init (pthread_barrier_t * barrier, + const pthread_barrierattr_t * attr, + unsigned int count); + +PTW32_DLLPORT int PTW32_CDECL pthread_barrier_destroy (pthread_barrier_t * barrier); + +PTW32_DLLPORT int PTW32_CDECL pthread_barrier_wait (pthread_barrier_t * barrier); + +/* + * Condition Variable Attribute Functions + */ +PTW32_DLLPORT int PTW32_CDECL pthread_condattr_init (pthread_condattr_t * attr); + +PTW32_DLLPORT int PTW32_CDECL pthread_condattr_destroy (pthread_condattr_t * attr); + +PTW32_DLLPORT int PTW32_CDECL pthread_condattr_getpshared (const pthread_condattr_t * attr, + int *pshared); + +PTW32_DLLPORT int PTW32_CDECL pthread_condattr_setpshared (pthread_condattr_t * attr, + int pshared); + +/* + * Condition Variable Functions + */ +PTW32_DLLPORT int PTW32_CDECL pthread_cond_init (pthread_cond_t * cond, + const pthread_condattr_t * attr); + +PTW32_DLLPORT int PTW32_CDECL pthread_cond_destroy (pthread_cond_t * cond); + +PTW32_DLLPORT int PTW32_CDECL pthread_cond_wait (pthread_cond_t * cond, + pthread_mutex_t * mutex); + +PTW32_DLLPORT int PTW32_CDECL pthread_cond_timedwait (pthread_cond_t * cond, + pthread_mutex_t * mutex, + const struct timespec *abstime); + +PTW32_DLLPORT int PTW32_CDECL pthread_cond_signal (pthread_cond_t * cond); + +PTW32_DLLPORT int PTW32_CDECL pthread_cond_broadcast (pthread_cond_t * cond); + +/* + * Scheduling + */ +PTW32_DLLPORT int PTW32_CDECL pthread_setschedparam (pthread_t thread, + int policy, + const struct sched_param *param); + +PTW32_DLLPORT int PTW32_CDECL pthread_getschedparam (pthread_t thread, + int *policy, + struct sched_param *param); + +PTW32_DLLPORT int PTW32_CDECL pthread_setconcurrency (int); + +PTW32_DLLPORT int PTW32_CDECL pthread_getconcurrency (void); + +/* + * Read-Write Lock Functions + */ +PTW32_DLLPORT int PTW32_CDECL pthread_rwlock_init(pthread_rwlock_t *lock, + const pthread_rwlockattr_t *attr); + +PTW32_DLLPORT int PTW32_CDECL pthread_rwlock_destroy(pthread_rwlock_t *lock); + +PTW32_DLLPORT int PTW32_CDECL pthread_rwlock_tryrdlock(pthread_rwlock_t *); + +PTW32_DLLPORT int PTW32_CDECL pthread_rwlock_trywrlock(pthread_rwlock_t *); + +PTW32_DLLPORT int PTW32_CDECL pthread_rwlock_rdlock(pthread_rwlock_t *lock); + +PTW32_DLLPORT int PTW32_CDECL pthread_rwlock_timedrdlock(pthread_rwlock_t *lock, + const struct timespec *abstime); + +PTW32_DLLPORT int PTW32_CDECL pthread_rwlock_wrlock(pthread_rwlock_t *lock); + +PTW32_DLLPORT int PTW32_CDECL pthread_rwlock_timedwrlock(pthread_rwlock_t *lock, + const struct timespec *abstime); + +PTW32_DLLPORT int PTW32_CDECL pthread_rwlock_unlock(pthread_rwlock_t *lock); + +PTW32_DLLPORT int PTW32_CDECL pthread_rwlockattr_init (pthread_rwlockattr_t * attr); + +PTW32_DLLPORT int PTW32_CDECL pthread_rwlockattr_destroy (pthread_rwlockattr_t * attr); + +PTW32_DLLPORT int PTW32_CDECL pthread_rwlockattr_getpshared (const pthread_rwlockattr_t * attr, + int *pshared); + +PTW32_DLLPORT int PTW32_CDECL pthread_rwlockattr_setpshared (pthread_rwlockattr_t * attr, + int pshared); + +#if PTW32_LEVEL >= PTW32_LEVEL_MAX - 1 + +/* + * Signal Functions. Should be defined in but MSVC and MinGW32 + * already have signal.h that don't define these. + */ +PTW32_DLLPORT int PTW32_CDECL pthread_kill(pthread_t thread, int sig); + +/* + * Non-portable functions + */ + +/* + * Compatibility with Linux. + */ +PTW32_DLLPORT int PTW32_CDECL pthread_mutexattr_setkind_np(pthread_mutexattr_t * attr, + int kind); +PTW32_DLLPORT int PTW32_CDECL pthread_mutexattr_getkind_np(pthread_mutexattr_t * attr, + int *kind); + +/* + * Possibly supported by other POSIX threads implementations + */ +PTW32_DLLPORT int PTW32_CDECL pthread_delay_np (struct timespec * interval); +PTW32_DLLPORT int PTW32_CDECL pthread_num_processors_np(void); +PTW32_DLLPORT unsigned __int64 PTW32_CDECL pthread_getunique_np(pthread_t thread); + +/* + * Useful if an application wants to statically link + * the lib rather than load the DLL at run-time. + */ +PTW32_DLLPORT int PTW32_CDECL pthread_win32_process_attach_np(void); +PTW32_DLLPORT int PTW32_CDECL pthread_win32_process_detach_np(void); +PTW32_DLLPORT int PTW32_CDECL pthread_win32_thread_attach_np(void); +PTW32_DLLPORT int PTW32_CDECL pthread_win32_thread_detach_np(void); + +/* + * Features that are auto-detected at load/run time. + */ +PTW32_DLLPORT int PTW32_CDECL pthread_win32_test_features_np(int); +enum ptw32_features { + PTW32_SYSTEM_INTERLOCKED_COMPARE_EXCHANGE = 0x0001, /* System provides it. */ + PTW32_ALERTABLE_ASYNC_CANCEL = 0x0002 /* Can cancel blocked threads. */ +}; + +/* + * Register a system time change with the library. + * Causes the library to perform various functions + * in response to the change. Should be called whenever + * the application's top level window receives a + * WM_TIMECHANGE message. It can be passed directly to + * pthread_create() as a new thread if desired. + */ +PTW32_DLLPORT void * PTW32_CDECL pthread_timechange_handler_np(void *); + +#endif /*PTW32_LEVEL >= PTW32_LEVEL_MAX - 1 */ + +#if PTW32_LEVEL >= PTW32_LEVEL_MAX + +/* + * Returns the Win32 HANDLE for the POSIX thread. + */ +PTW32_DLLPORT HANDLE PTW32_CDECL pthread_getw32threadhandle_np(pthread_t thread); +/* + * Returns the win32 thread ID for POSIX thread. + */ +PTW32_DLLPORT DWORD PTW32_CDECL pthread_getw32threadid_np (pthread_t thread); + + +/* + * Protected Methods + * + * This function blocks until the given WIN32 handle + * is signaled or pthread_cancel had been called. + * This function allows the caller to hook into the + * PThreads cancel mechanism. It is implemented using + * + * WaitForMultipleObjects + * + * on 'waitHandle' and a manually reset WIN32 Event + * used to implement pthread_cancel. The 'timeout' + * argument to TimedWait is simply passed to + * WaitForMultipleObjects. + */ +PTW32_DLLPORT int PTW32_CDECL pthreadCancelableWait (HANDLE waitHandle); +PTW32_DLLPORT int PTW32_CDECL pthreadCancelableTimedWait (HANDLE waitHandle, + DWORD timeout); + +#endif /* PTW32_LEVEL >= PTW32_LEVEL_MAX */ + +/* + * Thread-Safe C Runtime Library Mappings. + */ +#if !defined(_UWIN) +# if defined(NEED_ERRNO) + PTW32_DLLPORT int * PTW32_CDECL _errno( void ); +# else +# if !defined(errno) +# if (defined(_MT) || defined(_DLL)) + __declspec(dllimport) extern int * __cdecl _errno(void); +# define errno (*_errno()) +# endif +# endif +# endif +#endif + +/* + * Some compiler environments don't define some things. + */ +#if defined(__BORLANDC__) +# define _ftime ftime +# define _timeb timeb +#endif + +#if defined(__cplusplus) + +/* + * Internal exceptions + */ +class ptw32_exception {}; +class ptw32_exception_cancel : public ptw32_exception {}; +class ptw32_exception_exit : public ptw32_exception {}; + +#endif + +#if PTW32_LEVEL >= PTW32_LEVEL_MAX + +/* FIXME: This is only required if the library was built using SEH */ +/* + * Get internal SEH tag + */ +PTW32_DLLPORT DWORD PTW32_CDECL ptw32_get_exception_services_code(void); + +#endif /* PTW32_LEVEL >= PTW32_LEVEL_MAX */ + +#if !defined(PTW32_BUILD) + +#if defined(__CLEANUP_SEH) + +/* + * Redefine the SEH __except keyword to ensure that applications + * propagate our internal exceptions up to the library's internal handlers. + */ +#define __except( E ) \ + __except( ( GetExceptionCode() == ptw32_get_exception_services_code() ) \ + ? EXCEPTION_CONTINUE_SEARCH : ( E ) ) + +#endif /* __CLEANUP_SEH */ + +#if defined(__CLEANUP_CXX) + +/* + * Redefine the C++ catch keyword to ensure that applications + * propagate our internal exceptions up to the library's internal handlers. + */ +#if defined(_MSC_VER) + /* + * WARNING: Replace any 'catch( ... )' with 'PtW32CatchAll' + * if you want Pthread-Win32 cancelation and pthread_exit to work. + */ + +#if !defined(PtW32NoCatchWarn) + +#pragma message("Specify \"/DPtW32NoCatchWarn\" compiler flag to skip this message.") +#pragma message("------------------------------------------------------------------") +#pragma message("When compiling applications with MSVC++ and C++ exception handling:") +#pragma message(" Replace any 'catch( ... )' in routines called from POSIX threads") +#pragma message(" with 'PtW32CatchAll' or 'CATCHALL' if you want POSIX thread") +#pragma message(" cancelation and pthread_exit to work. For example:") +#pragma message("") +#pragma message(" #if defined(PtW32CatchAll)") +#pragma message(" PtW32CatchAll") +#pragma message(" #else") +#pragma message(" catch(...)") +#pragma message(" #endif") +#pragma message(" {") +#pragma message(" /* Catchall block processing */") +#pragma message(" }") +#pragma message("------------------------------------------------------------------") + +#endif + +#define PtW32CatchAll \ + catch( ptw32_exception & ) { throw; } \ + catch( ... ) + +#else /* _MSC_VER */ + +#define catch( E ) \ + catch( ptw32_exception & ) { throw; } \ + catch( E ) + +#endif /* _MSC_VER */ + +#endif /* __CLEANUP_CXX */ + +#endif /* ! PTW32_BUILD */ + +#if defined(__cplusplus) +} /* End of extern "C" */ +#endif /* __cplusplus */ + +#if defined(PTW32__HANDLE_DEF) +# undef HANDLE +#endif +#if defined(PTW32__DWORD_DEF) +# undef DWORD +#endif + +#undef PTW32_LEVEL +#undef PTW32_LEVEL_MAX + +#endif /* ! RC_INVOKED */ + +#endif /* PTHREAD_H */ diff --git a/lwrserv/pthread/pthreadVC2.dll b/lwrserv/pthread/pthreadVC2.dll new file mode 100755 index 0000000..fcb5d9d Binary files /dev/null and b/lwrserv/pthread/pthreadVC2.dll differ diff --git a/lwrserv/pthread/pthreadVC2.lib b/lwrserv/pthread/pthreadVC2.lib new file mode 100755 index 0000000..c20ee20 Binary files /dev/null and b/lwrserv/pthread/pthreadVC2.lib differ diff --git a/lwrserv/pthread/pthreadVCE2.dll b/lwrserv/pthread/pthreadVCE2.dll new file mode 100755 index 0000000..9d148cc Binary files /dev/null and b/lwrserv/pthread/pthreadVCE2.dll differ diff --git a/lwrserv/pthread/pthreadVCE2.lib b/lwrserv/pthread/pthreadVCE2.lib new file mode 100755 index 0000000..7f05317 Binary files /dev/null and b/lwrserv/pthread/pthreadVCE2.lib differ diff --git a/lwrserv/pthread/pthreadVSE2.dll b/lwrserv/pthread/pthreadVSE2.dll new file mode 100755 index 0000000..8129116 Binary files /dev/null and b/lwrserv/pthread/pthreadVSE2.dll differ diff --git a/lwrserv/pthread/pthreadVSE2.lib b/lwrserv/pthread/pthreadVSE2.lib new file mode 100755 index 0000000..3f3335d Binary files /dev/null and b/lwrserv/pthread/pthreadVSE2.lib differ diff --git a/lwrserv/pthread/sched.h b/lwrserv/pthread/sched.h new file mode 100755 index 0000000..f36a97a --- /dev/null +++ b/lwrserv/pthread/sched.h @@ -0,0 +1,183 @@ +/* + * Module: sched.h + * + * Purpose: + * Provides an implementation of POSIX realtime extensions + * as defined in + * + * POSIX 1003.1b-1993 (POSIX.1b) + * + * -------------------------------------------------------------------------- + * + * Pthreads-win32 - POSIX Threads Library for Win32 + * Copyright(C) 1998 John E. Bossom + * Copyright(C) 1999,2005 Pthreads-win32 contributors + * + * Contact Email: rpj@callisto.canberra.edu.au + * + * The current list of contributors is contained + * in the file CONTRIBUTORS included with the source + * code distribution. The list can also be seen at the + * following World Wide Web location: + * http://sources.redhat.com/pthreads-win32/contributors.html + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2 of the License, or (at your option) any later version. + * + * This library 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 + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library in the file COPYING.LIB; + * if not, write to the Free Software Foundation, Inc., + * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA + */ +#if !defined(_SCHED_H) +#define _SCHED_H + +#undef PTW32_SCHED_LEVEL + +#if defined(_POSIX_SOURCE) +#define PTW32_SCHED_LEVEL 0 +/* Early POSIX */ +#endif + +#if defined(_POSIX_C_SOURCE) && _POSIX_C_SOURCE >= 199309 +#undef PTW32_SCHED_LEVEL +#define PTW32_SCHED_LEVEL 1 +/* Include 1b, 1c and 1d */ +#endif + +#if defined(INCLUDE_NP) +#undef PTW32_SCHED_LEVEL +#define PTW32_SCHED_LEVEL 2 +/* Include Non-Portable extensions */ +#endif + +#define PTW32_SCHED_LEVEL_MAX 3 + +#if ( defined(_POSIX_C_SOURCE) && _POSIX_C_SOURCE >= 200112 ) || !defined(PTW32_SCHED_LEVEL) +#define PTW32_SCHED_LEVEL PTW32_SCHED_LEVEL_MAX +/* Include everything */ +#endif + + +#if defined(__GNUC__) && !defined(__declspec) +# error Please upgrade your GNU compiler to one that supports __declspec. +#endif + +/* + * When building the library, you should define PTW32_BUILD so that + * the variables/functions are exported correctly. When using the library, + * do NOT define PTW32_BUILD, and then the variables/functions will + * be imported correctly. + */ +#if !defined(PTW32_STATIC_LIB) +# if defined(PTW32_BUILD) +# define PTW32_DLLPORT __declspec (dllexport) +# else +# define PTW32_DLLPORT __declspec (dllimport) +# endif +#else +# define PTW32_DLLPORT +#endif + +/* + * This is a duplicate of what is in the autoconf config.h, + * which is only used when building the pthread-win32 libraries. + */ + +#if !defined(PTW32_CONFIG_H) +# if defined(WINCE) +# define NEED_ERRNO +# define NEED_SEM +# endif +# if defined(__MINGW64__) +# define HAVE_STRUCT_TIMESPEC +# define HAVE_MODE_T +# elif defined(_UWIN) || defined(__MINGW32__) +# define HAVE_MODE_T +# endif +#endif + +/* + * + */ + +#if PTW32_SCHED_LEVEL >= PTW32_SCHED_LEVEL_MAX +#if defined(NEED_ERRNO) +#include "need_errno.h" +#else +#include +#endif +#endif /* PTW32_SCHED_LEVEL >= PTW32_SCHED_LEVEL_MAX */ + +#if (defined(__MINGW64__) || defined(__MINGW32__)) || defined(_UWIN) +# if PTW32_SCHED_LEVEL >= PTW32_SCHED_LEVEL_MAX +/* For pid_t */ +# include +/* Required by Unix 98 */ +# include +# else + typedef int pid_t; +# endif +#else + typedef int pid_t; +#endif + +/* Thread scheduling policies */ + +enum { + SCHED_OTHER = 0, + SCHED_FIFO, + SCHED_RR, + SCHED_MIN = SCHED_OTHER, + SCHED_MAX = SCHED_RR +}; + +struct sched_param { + int sched_priority; +}; + +#if defined(__cplusplus) +extern "C" +{ +#endif /* __cplusplus */ + +PTW32_DLLPORT int __cdecl sched_yield (void); + +PTW32_DLLPORT int __cdecl sched_get_priority_min (int policy); + +PTW32_DLLPORT int __cdecl sched_get_priority_max (int policy); + +PTW32_DLLPORT int __cdecl sched_setscheduler (pid_t pid, int policy); + +PTW32_DLLPORT int __cdecl sched_getscheduler (pid_t pid); + +/* + * Note that this macro returns ENOTSUP rather than + * ENOSYS as might be expected. However, returning ENOSYS + * should mean that sched_get_priority_{min,max} are + * not implemented as well as sched_rr_get_interval. + * This is not the case, since we just don't support + * round-robin scheduling. Therefore I have chosen to + * return the same value as sched_setscheduler when + * SCHED_RR is passed to it. + */ +#define sched_rr_get_interval(_pid, _interval) \ + ( errno = ENOTSUP, (int) -1 ) + + +#if defined(__cplusplus) +} /* End of extern "C" */ +#endif /* __cplusplus */ + +#undef PTW32_SCHED_LEVEL +#undef PTW32_SCHED_LEVEL_MAX + +#endif /* !_SCHED_H */ + diff --git a/lwrserv/pthread/semaphore.h b/lwrserv/pthread/semaphore.h new file mode 100755 index 0000000..c6e9407 --- /dev/null +++ b/lwrserv/pthread/semaphore.h @@ -0,0 +1,169 @@ +/* + * Module: semaphore.h + * + * Purpose: + * Semaphores aren't actually part of the PThreads standard. + * They are defined by the POSIX Standard: + * + * POSIX 1003.1b-1993 (POSIX.1b) + * + * -------------------------------------------------------------------------- + * + * Pthreads-win32 - POSIX Threads Library for Win32 + * Copyright(C) 1998 John E. Bossom + * Copyright(C) 1999,2005 Pthreads-win32 contributors + * + * Contact Email: rpj@callisto.canberra.edu.au + * + * The current list of contributors is contained + * in the file CONTRIBUTORS included with the source + * code distribution. The list can also be seen at the + * following World Wide Web location: + * http://sources.redhat.com/pthreads-win32/contributors.html + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2 of the License, or (at your option) any later version. + * + * This library 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 + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library in the file COPYING.LIB; + * if not, write to the Free Software Foundation, Inc., + * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA + */ +#if !defined( SEMAPHORE_H ) +#define SEMAPHORE_H + +#undef PTW32_SEMAPHORE_LEVEL + +#if defined(_POSIX_SOURCE) +#define PTW32_SEMAPHORE_LEVEL 0 +/* Early POSIX */ +#endif + +#if defined(_POSIX_C_SOURCE) && _POSIX_C_SOURCE >= 199309 +#undef PTW32_SEMAPHORE_LEVEL +#define PTW32_SEMAPHORE_LEVEL 1 +/* Include 1b, 1c and 1d */ +#endif + +#if defined(INCLUDE_NP) +#undef PTW32_SEMAPHORE_LEVEL +#define PTW32_SEMAPHORE_LEVEL 2 +/* Include Non-Portable extensions */ +#endif + +#define PTW32_SEMAPHORE_LEVEL_MAX 3 + +#if !defined(PTW32_SEMAPHORE_LEVEL) +#define PTW32_SEMAPHORE_LEVEL PTW32_SEMAPHORE_LEVEL_MAX +/* Include everything */ +#endif + +#if defined(__GNUC__) && ! defined (__declspec) +# error Please upgrade your GNU compiler to one that supports __declspec. +#endif + +/* + * When building the library, you should define PTW32_BUILD so that + * the variables/functions are exported correctly. When using the library, + * do NOT define PTW32_BUILD, and then the variables/functions will + * be imported correctly. + */ +#if !defined(PTW32_STATIC_LIB) +# if defined(PTW32_BUILD) +# define PTW32_DLLPORT __declspec (dllexport) +# else +# define PTW32_DLLPORT __declspec (dllimport) +# endif +#else +# define PTW32_DLLPORT +#endif + +/* + * This is a duplicate of what is in the autoconf config.h, + * which is only used when building the pthread-win32 libraries. + */ + +#if !defined(PTW32_CONFIG_H) +# if defined(WINCE) +# define NEED_ERRNO +# define NEED_SEM +# endif +# if defined(__MINGW64__) +# define HAVE_STRUCT_TIMESPEC +# define HAVE_MODE_T +# elif defined(_UWIN) || defined(__MINGW32__) +# define HAVE_MODE_T +# endif +#endif + +/* + * + */ + +#if PTW32_SEMAPHORE_LEVEL >= PTW32_SEMAPHORE_LEVEL_MAX +#if defined(NEED_ERRNO) +#include "need_errno.h" +#else +#include +#endif +#endif /* PTW32_SEMAPHORE_LEVEL >= PTW32_SEMAPHORE_LEVEL_MAX */ + +#define _POSIX_SEMAPHORES + +#if defined(__cplusplus) +extern "C" +{ +#endif /* __cplusplus */ + +#if !defined(HAVE_MODE_T) +typedef unsigned int mode_t; +#endif + + +typedef struct sem_t_ * sem_t; + +PTW32_DLLPORT int __cdecl sem_init (sem_t * sem, + int pshared, + unsigned int value); + +PTW32_DLLPORT int __cdecl sem_destroy (sem_t * sem); + +PTW32_DLLPORT int __cdecl sem_trywait (sem_t * sem); + +PTW32_DLLPORT int __cdecl sem_wait (sem_t * sem); + +PTW32_DLLPORT int __cdecl sem_timedwait (sem_t * sem, + const struct timespec * abstime); + +PTW32_DLLPORT int __cdecl sem_post (sem_t * sem); + +PTW32_DLLPORT int __cdecl sem_post_multiple (sem_t * sem, + int count); + +PTW32_DLLPORT int __cdecl sem_open (const char * name, + int oflag, + mode_t mode, + unsigned int value); + +PTW32_DLLPORT int __cdecl sem_close (sem_t * sem); + +PTW32_DLLPORT int __cdecl sem_unlink (const char * name); + +PTW32_DLLPORT int __cdecl sem_getvalue (sem_t * sem, + int * sval); + +#if defined(__cplusplus) +} /* End of extern "C" */ +#endif /* __cplusplus */ + +#undef PTW32_SEMAPHORE_LEVEL +#undef PTW32_SEMAPHORE_LEVEL_MAX + +#endif /* !SEMAPHORE_H */ diff --git a/lwrserv/resource.h b/lwrserv/resource.h new file mode 100755 index 0000000..fe9f646 --- /dev/null +++ b/lwrserv/resource.h @@ -0,0 +1,14 @@ +//{{NO_DEPENDENCIES}} +// Microsoft Visual C++ generated include file. +// Used by lwrserv.rc + +// Nächste Standardwerte für neue Objekte +// +#ifdef APSTUDIO_INVOKED +#ifndef APSTUDIO_READONLY_SYMBOLS +#define _APS_NEXT_RESOURCE_VALUE 101 +#define _APS_NEXT_COMMAND_VALUE 40001 +#define _APS_NEXT_CONTROL_VALUE 1001 +#define _APS_NEXT_SYMED_VALUE 101 +#endif +#endif diff --git a/lwrserv/sgn.h b/lwrserv/sgn.h new file mode 100755 index 0000000..52052d4 --- /dev/null +++ b/lwrserv/sgn.h @@ -0,0 +1,13 @@ +#ifndef __SGN_H__ +#define __SGN_H__ +inline float sgn(float x) +{ + if ( x == 0 ) + return 0.0f; + + if ( x > 0) + return 1.0f; + else + return -1.0f; +} +#endif diff --git a/lwrserv/vec.h b/lwrserv/vec.h new file mode 100755 index 0000000..36636e3 --- /dev/null +++ b/lwrserv/vec.h @@ -0,0 +1,234 @@ +#ifndef _VEC_H_ +#define _VEC_H_ +#include +#include "mat.h" +#include + +template class Mat; +template class Vec; + +// Vector class for SIMPLE data types +template class Vec +{ +public: + + // standard constructor + Vec () + { // initialize all elements with zero + for (unsigned int i=0; i (const T atData[SIZE]) + { + for (unsigned int i=0; i (const Vec &vec) + { + if (this==&vec) + return; // nothing to do, it's me + for (unsigned int i=0; i &operator = (const Vec &vec) + { + if (this==&vec) + return (*this); // ok, it's me, so no L-value action + for (unsigned int i=0; i &operator = (const T atData[SIZE]) + { + for (unsigned int i=0; i