From bdfe27de8ff01ac02852b19fc3c94f566f32fcd6 Mon Sep 17 00:00:00 2001 From: philipp schoenberger Date: Tue, 18 Aug 2015 21:55:51 +0200 Subject: [PATCH] improve command call structure by function point + structs --- lwrserv/SocketObject.cpp | 5 - lwrserv/SvrHandling.cpp | 309 +++++++++++++++++++++++---------------- lwrserv/SvrHandling.h | 109 +++++++------- 3 files changed, 233 insertions(+), 190 deletions(-) diff --git a/lwrserv/SocketObject.cpp b/lwrserv/SocketObject.cpp index 3415a04..91f362b 100644 --- a/lwrserv/SocketObject.cpp +++ b/lwrserv/SocketObject.cpp @@ -170,7 +170,6 @@ bool SocketObject::Accept(SocketObject &skAcceptSocket) #endif } - bool SocketObject::Connect(const char* szServerAddress, unsigned short int iPort, bool nonblock) { #if defined(__msdos__) || defined(WIN32) @@ -248,13 +247,11 @@ bool SocketObject::Connect(const char* szServerAddress, unsigned short int iPort #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) @@ -310,7 +307,6 @@ int SocketObject::Recv (string& s) return status; } - int SocketObject::receiveLine(char* array, int arraySize) { //std::string ret; @@ -384,7 +380,6 @@ 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; diff --git a/lwrserv/SvrHandling.cpp b/lwrserv/SvrHandling.cpp index 0e33396..2ce5368 100755 --- a/lwrserv/SvrHandling.cpp +++ b/lwrserv/SvrHandling.cpp @@ -13,37 +13,16 @@ #include "LinearTrajectory.h" #include -void printUsage(SocketObject& client) +void printUsage(SocketObject& client, std::string& arg) { - client.Send(SVR_HELP_MSG); - -} - -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) + (void) arg; + for (unsigned int i = 0 ; i < commandCount; ++i) { - sa = M[1][0]/norm; - ca = M[0][0]/norm; - a = atan2(sa,ca); - } else { - sa = 0; - ca = 1; - a = 0; + if(commands[i].printHelp != NULL) + commands[i].printHelp(); + else + client.Send(commands[i].aberration+"\t-\t"+commands[i].longVersion); } - 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) -{ - cout << M << "\n\r" << endl; } VecJoint kukaBackwardCalc(double M_[12], float arg[3]) @@ -316,6 +295,96 @@ Mat4f getTranslation(double tx, double ty, double tz) SvrHandling::SvrHandling() { + // add each command which is possible + commands = new ClientCommand[20]; + + int i = 0; + commands[i].aberration = "GPJ"; + commands[i].longVersion = "GetPositionJoints"; + commands[i].processCommand = &getPositionJoints; + commands[i].printHelp = NULL; + i+=1; + commands[i].aberration = "GPHRW"; + commands[i].longVersion = "GetPositionHomRowWise"; + commands[i].processCommand = &getPositionHomRowWise; + commands[i].printHelp = NULL; + i+=1; + commands[i].aberration = "GFT"; + commands[i].longVersion = "GetForceTorqueTcp"; + commands[i].processCommand = &getForceTorqueTcp; + commands[i].printHelp = NULL; + i+=1; + commands[i].aberration = "MPTPJ"; + commands[i].longVersion = "MovePTPJoints"; + commands[i].processCommand = &movePTPJoints; + commands[i].printHelp = NULL; + i+=1; + commands[i].aberration = "MHRWS"; + commands[i].longVersion = "MoveHomRowWiseStatus"; + commands[i].processCommand = &moveHomRowWiseStatus; + commands[i].printHelp = NULL; + i+=1; + commands[i].aberration = "SS"; + commands[i].longVersion = "SetSpeed"; + commands[i].processCommand = &setSpeed; + commands[i].printHelp = NULL; + i+=1; + commands[i].aberration = "SA"; + commands[i].longVersion = "SetAccel"; + commands[i].processCommand = &setAccel; + commands[i].printHelp = NULL; + i+=1; + commands[i].aberration = "STPF"; + commands[i].longVersion = "StartPotFieldMode"; + commands[i].processCommand = &startPotFieldMode; + commands[i].printHelp = NULL; + i+=1; + commands[i].aberration = "SPPF"; + commands[i].longVersion = "StopPotFieldMode"; + commands[i].processCommand = &stopPotFieldMode; + commands[i].printHelp = NULL; + i+=1; + commands[i].aberration = "SP"; + commands[i].longVersion = "SetPos"; + commands[i].processCommand = &setPos; + commands[i].printHelp = NULL; + i+=1; + commands[i].aberration = "SJ"; + commands[i].longVersion = "SetJoints"; + commands[i].processCommand = &setJoints; + commands[i].printHelp = NULL; + i+=1; + commands[i].aberration = "Q"; + commands[i].longVersion = "Quit"; + commands[i].processCommand = &quit; + commands[i].printHelp = NULL; + i+=1; + commands[i].aberration = "?"; + commands[i].longVersion = "Help"; + commands[i].processCommand = &printUsage; + commands[i].printHelp = NULL; + i+=1; + commands[i].aberration = "ISKUKA"; + commands[i].longVersion = "IsKukaLWR"; + commands[i].processCommand = NULL; + commands[i].printHelp = NULL; + i+=1; + commands[i].aberration = "MC"; + commands[i].longVersion = "MoveCartesian"; + commands[i].processCommand = &moveCartesian; + commands[i].printHelp = NULL; + i+=1; + commands[i].aberration = "GT"; + commands[i].longVersion = "GetTrajectoryType"; + commands[i].processCommand = &getTrajectoryType; + commands[i].printHelp = NULL; + i+=1; + commands[i].aberration = "ST"; + commands[i].longVersion = "SetTrajectoryType"; + commands[i].processCommand = &setTrajectoryType; + commands[i].printHelp = NULL; + + commandCount = i; } SvrHandling::~SvrHandling() @@ -392,71 +461,35 @@ void SvrHandling::clientCommandLoop(SocketObject& client) cout<getMessuredCartPos(); - // assemble command feadback row wise + // assemble command feedback row wise for (int i=0; i< FRI_CART_FRM_DIM; i++) { int row = i % 4; @@ -524,8 +562,11 @@ void SvrHandling::GetPositionHomRowWise(SocketObject& client) client.Send(out); } -void SvrHandling::GetForceTorqueTcp(SocketObject& client) +void getForceTorqueTcp(SocketObject& client, std::string& arg) { + // unused + (void) arg; + string out = ""; stringstream ss (stringstream::in | stringstream::out); @@ -539,11 +580,11 @@ void SvrHandling::GetForceTorqueTcp(SocketObject& client) client.Send(out); } -void SvrHandling::MovePTPJoints(SocketObject& client, string& args) +void movePTPJoints(SocketObject& client, string& arg) { string buf; - stringstream ss(args); + stringstream ss(arg); stringstream ss2f; // convert string to joint vector @@ -604,11 +645,11 @@ void SvrHandling::MovePTPJoints(SocketObject& client, string& args) client.Send(SVR_TRUE_RSP); } -void SvrHandling::MoveHomRowWiseStatus(SocketObject& client, string& args) +void moveHomRowWiseStatus(SocketObject& client, string& arg) { string buf; double temp[15]; - stringstream ss(args); + stringstream ss(arg); stringstream ss2f; int argc = 0; @@ -647,14 +688,14 @@ void SvrHandling::MoveHomRowWiseStatus(SocketObject& client, string& args) } // extract elbow flip and hand parameter - float arg[3]; + float configurationParameter[3]; for (int i=0;i<3;i++) { - arg[i] = temp[12+i]; + configurationParameter[i] = temp[12+i]; } //Backward Calculation - VecJoint newJointPos = kukaBackwardCalc(temp, arg); + VecJoint newJointPos = kukaBackwardCalc(temp, configurationParameter); //Check for Joint Range @@ -691,17 +732,17 @@ void SvrHandling::MoveHomRowWiseStatus(SocketObject& client, string& args) client.Send(SVR_TRUE_RSP); } -void SvrHandling::SetSpeed(SocketObject& client, string& args) +void setSpeed(SocketObject& client, string& arg) { string buf; float newVelocityPercentage; int argc = 0; - stringstream ss(args); + stringstream ss(arg); stringstream ss2f; vector tokens; // no speed argument - if (args == "") + if (arg == "") { client.Send(SVR_FALSE_RSP); return; @@ -737,16 +778,16 @@ void SvrHandling::SetSpeed(SocketObject& client, string& args) client.Send(SVR_TRUE_RSP); } -void SvrHandling::SetAccel(SocketObject& client, string& args) +void setAccel(SocketObject& client, string& arg) { string buf; float newAccelerationPercentage; - stringstream ss(args); + stringstream ss(arg); stringstream ss2f; int argc = 0; // need at least one argument - if (args == "") + if (arg == "") { client.Send(SVR_FALSE_RSP); return; @@ -782,8 +823,10 @@ void SvrHandling::SetAccel(SocketObject& client, string& args) client.Send(SVR_TRUE_RSP); } -void SvrHandling::StartPotFieldMode(SocketObject& client, string& args) +void startPotFieldMode(SocketObject& client, string& arg) { + // unused + (void) arg; #if 0 __MSRSTARTPOTFIELD = false; __MSRSTOPPOTFIELD = false; @@ -804,8 +847,10 @@ void SvrHandling::StartPotFieldMode(SocketObject& client, string& args) client.Send(SVR_TRUE_RSP); } -void SvrHandling::StopPotFieldMode(SocketObject& client, string& args) +void stopPotFieldMode(SocketObject& client, string& arg) { + // unused + (void) arg; #if 0 __POTFIELDMODE = false; @@ -820,11 +865,11 @@ void SvrHandling::StopPotFieldMode(SocketObject& client, string& args) client.Send(SVR_TRUE_RSP); } -void SvrHandling::SetPos(SocketObject& client, string& args) +void setPos(SocketObject& client, string& arg) { string buf; double temp[15]; - stringstream ss(args); + stringstream ss(arg); stringstream ss2f; int argc = 0; @@ -864,14 +909,14 @@ void SvrHandling::SetPos(SocketObject& client, string& args) } // extract elbow flip and hand parameter - float arg[3]; + float configurationParameter[3]; for (int i=0;i<3;i++) { - arg[i] = temp[12+i]; + configurationParameter[i] = temp[12+i]; } //Backward Calculation of the position - VecJoint CalcJoints = kukaBackwardCalc(temp, arg); + VecJoint CalcJoints = kukaBackwardCalc(temp, configurationParameter); //Check for a valid joint range if (!SvrData::getInstance()->checkJointRange(CalcJoints)) @@ -891,19 +936,19 @@ void SvrHandling::SetPos(SocketObject& client, string& args) client.Send(SVR_TRUE_RSP); } -void SvrHandling::SetJoints(SocketObject& client, string& args) +void setJoints(SocketObject& client, string& arg) { string buf; VecJoint newJointPos(0.0f); - stringstream ss(args); + stringstream ss(arg); stringstream ss2f; - int i = 0; + int argc = 0; // convert to Joint vector while (ss >> buf) { // to many input arguments or Joints - if (i>=JOINT_NUMBER) + if (argc>=JOINT_NUMBER) { client.Send(SVR_INVALID_ARGUMENT_COUNT); return; @@ -913,12 +958,12 @@ void SvrHandling::SetJoints(SocketObject& client, string& args) ss2f.flush(); ss2f.clear(); ss2f << buf; - ss2f >> newJointPos(i); - i++; + ss2f >> newJointPos(argc); + argc++; } // to less joint values - if (i != JOINT_NUMBER) + if (argc != JOINT_NUMBER) { client.Send(SVR_INVALID_ARGUMENT_COUNT); return; @@ -946,11 +991,11 @@ void SvrHandling::SetJoints(SocketObject& client, string& args) client.Send(SVR_TRUE_RSP); } -void SvrHandling::MoveCartesian(SocketObject& client, string& args) +void moveCartesian(SocketObject& client, string& arg) { string buf; float temp[15]; - stringstream ss(args); + stringstream ss(arg); stringstream ss2f; int argc = 0; @@ -977,10 +1022,10 @@ void SvrHandling::MoveCartesian(SocketObject& client, string& args) return ; } - float arg[3]; + float configurationParameter[3]; for (int i=0;i<3;i++) { - arg[i] = temp[12+i]; + configurationParameter[i] = temp[12+i]; } //Calculating end-effector position for target "TRobot" @@ -1005,12 +1050,16 @@ void SvrHandling::MoveCartesian(SocketObject& client, string& args) client.Send(SVR_TRUE_RSP); } -void SvrHandling::quit(SocketObject& client) +void quit(SocketObject& client , std::string& arg) { + //unused + (void) arg; + client.Send(SVR_TRUE_RSP); client.Disconnect(); } -void SvrHandling::setTrajectoryType(SocketObject& client, string& arg) + +void setTrajectoryType(SocketObject& client, std::string& arg) { string buf; enum TrajectoryType newType = TrajectoryDefault; @@ -1021,7 +1070,7 @@ void SvrHandling::setTrajectoryType(SocketObject& client, string& arg) client.Send(SVR_TRUE_RSP); } -void SvrHandling::getTrajectoryType(SocketObject& client) +void getTrajectoryType(SocketObject& client, std::string& arg) { enum TrajectoryType currType = SvrData::getInstance()->getTrajectoryType(); cout << "curr trajectory type : "<< toString(currType) << "\n"; @@ -1029,7 +1078,7 @@ void SvrHandling::getTrajectoryType(SocketObject& client) client.Send("current trajectory: " + toString(currType)); } -void SvrHandling::debug(SocketObject& client) +void debug(SocketObject& client, std::string& arg) { //check = true; diff --git a/lwrserv/SvrHandling.h b/lwrserv/SvrHandling.h index 3792640..cd6e583 100755 --- a/lwrserv/SvrHandling.h +++ b/lwrserv/SvrHandling.h @@ -10,38 +10,19 @@ #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 + ("!") + (" Type \"")+ SVR_HANDSHAKE + ("\" to establish the connection") #define SVR_ACCEPTED std::string("accepted") #define SVR_FAILED std::string("Failed.") #define SVR_TRUE_RSP "true" #define SVR_FALSE_RSP "false" + // server error messages #define SVR_INVALID_ARGUMENT "ERROR: The argument is not correct." +#define SVR_UNIMPLEMENTED_COMMAND "ERROR: This command is jet not implemented." +#define SVR_UNKNOWN_COMMAND "ERROR: This command is not correct." #define SVR_INVALID_ARGUMENT_COUNT "ERROR: The argument count is not correct." #define SVR_OUT_OF_RANGE "ERROR: At least one argument is out of Range." -#define SVR_HELP_MSG "GPJ - GetPositionJoints " - -//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_QUIT1 "quit" -#define CMD_QUIT2 "exit" -#define CMD_HELP "help" -#define CMD_ISKUKA "IsKukaLWR" -#define CMD_MoveCartesian "MC" -#define CMD_SET_TRAJECTORY_TYPE "STRAJ" -#define CMD_GET_TRAJECTORY_TYPE "GTRAJ" -//End SVR_COMMANDS typedef enum {waiting, handshake, accepted, done} svr_state; @@ -53,38 +34,11 @@ private: bool handshakeAccepted(SocketObject& client); //handle client commands he is disconected void clientCommandLoop(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); - // set a specific trajectory type - void setTrajectoryType(SocketObject& client, std::string& arg); - void getTrajectoryType(SocketObject& client); - //Quit - void quit(SocketObject& client); - //DEBUGGING - void debug(SocketObject& client); + + + + + public: //Constructor SvrHandling(); @@ -96,5 +50,50 @@ public: void run(int port); //Get current timestamp std::string timestamp(); + +}; + +struct ClientCommand +{ + std::string aberration; + std::string longVersion; + void (*processCommand) ( SocketObject& client, std::string& argv); + void (*printHelp) (void); }; +static struct ClientCommand* commands; +static unsigned int commandCount; + +void printUsage(SocketObject& client, std::string& arg); +//Handling request for current Joint Values +void getPositionJoints(SocketObject& client, std::string& arg); +//Get Position as POSE Matrix +void getPositionHomRowWise(SocketObject& client, std::string& arg); +//Get Force/torque values from TCP +void getForceTorqueTcp(SocketObject& client, std::string& arg); +//Move to given Joint combination +void movePTPJoints(SocketObject& client, std::string& arg); +//Move to given POSE position +void moveHomRowWiseStatus(SocketObject& client, std::string& arg); +//Set Velocity +void setSpeed(SocketObject& client, std::string& arg); +//Set Acceleration +void setAccel(SocketObject& client, std::string& arg); +//Starting Potential Field Movement Mode +void startPotFieldMode(SocketObject& client, std::string& arg); +//Stopping Potential Field Movement Mode +void stopPotFieldMode(SocketObject& client, std::string& arg); +//Setting Target Position HomRowWise for Potential Move +void setPos(SocketObject& client, std::string& arg); +//Setting Target Position as Joints for Potential Move +void setJoints(SocketObject& client, std::string& arg); +//Cartesian Movement +//Move to given POSE position +void moveCartesian(SocketObject& client, std::string& arg); +// set a specific trajectory type +void setTrajectoryType(SocketObject& client, std::string& arg); +void getTrajectoryType(SocketObject& client, std::string& arg); +//Quit +void quit(SocketObject& client, std::string& arg); +//DEBUGGING +void debug(SocketObject& client, std::string& arg); #endif