From 6e9a8228d48d71ac863490025820662429b9a2bb Mon Sep 17 00:00:00 2001 From: philipp schoenberger Date: Mon, 24 Aug 2015 18:39:54 +0200 Subject: [PATCH] move thread to svrHandling --- lwrserv/include/SvrHandling.h | 6 +- lwrserv/src/SvrData.cpp | 6 +- lwrserv/src/SvrHandling.cpp | 144 +++++++++++++++++++++++++++++++++ lwrserv/src/main.cpp | 146 +--------------------------------- 4 files changed, 151 insertions(+), 151 deletions(-) diff --git a/lwrserv/include/SvrHandling.h b/lwrserv/include/SvrHandling.h index 5381fe8..93f23b0 100644 --- a/lwrserv/include/SvrHandling.h +++ b/lwrserv/include/SvrHandling.h @@ -38,10 +38,6 @@ private: //handle client commands he is disconected void clientCommandLoop(SocketObject& client); - - - - public: //Constructor SvrHandling(); @@ -53,6 +49,8 @@ public: void run(int port); //Get current timestamp std::string timestamp(); + + static void * threadRobotMovement(void *arg); }; diff --git a/lwrserv/src/SvrData.cpp b/lwrserv/src/SvrData.cpp index 2c9c31c..74084f8 100644 --- a/lwrserv/src/SvrData.cpp +++ b/lwrserv/src/SvrData.cpp @@ -450,7 +450,7 @@ class Trajectory* SvrData::createTrajectory(Robot::VecJoint newJo //set new target for next trajectory Robot::VecJoint prevJointPos = getCommandedJointPos(); class Trajectory* newTrajectory; - + float sampleTimeMs = getSampleTimeMs(); Robot::VecJoint maxJointVelocity = getRobotVelocity(); Robot::VecJoint maxJointAcceleration = getRobotAcceleration(); @@ -463,7 +463,9 @@ class Trajectory* SvrData::createTrajectory(Robot::VecJoint newJo } class Trajectory* SvrData::createTrajectory(MatCarthesian newJointPos) { - + (void) newJointPos; + // unimplemented + return NULL; } Robot::VecJoint SvrData::getMeasuredJointPos() { diff --git a/lwrserv/src/SvrHandling.cpp b/lwrserv/src/SvrHandling.cpp index 23ac9e0..4662606 100644 --- a/lwrserv/src/SvrHandling.cpp +++ b/lwrserv/src/SvrHandling.cpp @@ -105,6 +105,150 @@ SvrHandling::SvrHandling() commandCount = i; } +void * SvrHandling::threadRobotMovement(void *arg) +{ + // unused parameters + (void) arg; + + Trajectory* currentTrajectory = NULL; + enum MovementType currentMovementType = MovementJointBased; + + Robot::VecJoint currentJointPos(0.0f); + MatCarthesian currentCartPos(0.0f,1.0f); + + friRemote* friInst = SvrData::getInstance()->getFriRemote(); + + // get current robot position + int tries = 2; + + for (int i= 0 ; i < tries ; ++i) + { + if (REAL_ROBOT) + SvrData::getInstance()->updateMessurement(); + currentJointPos = SvrData::getInstance()->getMeasuredJointPos(); + + SvrData::getInstance()->setCommandedJointPos(currentJointPos); + + float newJointPosToSend[Robot::joints] = {0.0f}; + currentJointPos.getData(newJointPosToSend); + if (REAL_ROBOT) + friInst->doPositionControl(newJointPosToSend); + } + std::cout << "init position is " << currentJointPos << "\n"; + + while(true) + { + if (REAL_ROBOT) + SvrData::getInstance()->updateMessurement(); + + bool positionChanged = false; + // check if we have to cancel current trajectory + bool cancel = SvrData::getInstance()->getTrajectoryCancel(); + if (cancel) + { + // free the current trajectory + if ( currentTrajectory != NULL) + { + delete currentTrajectory; + currentTrajectory = NULL; + } + // signal waiting thread the cancellation + SvrData::getInstance()->trajectoryCancelDone(); + goto end; + } + + // check for new trajectory + if (currentTrajectory == NULL) + { + // get next trajectory from svrData + currentTrajectory = SvrData::getInstance()->popTrajectory(); + + // no new trajectory + if(currentTrajectory == NULL) + { + // mark that we reached the end of the trajectory + // stay on current position + goto end; + } + else + { + std::cout <<"new Trajectory with " << currentTrajectory->getSteps()<<"\n"; + } + } + + // get next commanded trajectory type + if (currentTrajectory != NULL) + { + switch (currentTrajectory->getMovementType()) + { + case MovementCartBased: + { + currentCartPos = currentTrajectory->getNextCartPos(); + positionChanged = true; + currentMovementType = MovementCartBased; + } + break; + case MovementJointBased: + { + currentJointPos = currentTrajectory->getNextJointPos(); + positionChanged = true; + currentMovementType = MovementJointBased; + } + break; + default: + { + // invalid trajectory skip it + delete currentTrajectory; + currentTrajectory = NULL; + goto end; + } + break; + } + } + +end: + switch (currentMovementType) + { + case MovementJointBased: + { + // send the new joint values + float newJointPosToSend[Robot::joints] = {0.0f}; + Robot::VecJoint newJointPosToSendRad = currentJointPos*(1./180*M_PI); + newJointPosToSendRad.getData(newJointPosToSend); + if (REAL_ROBOT) + friInst->doPositionControl(newJointPosToSend); + } + break; + case MovementCartBased: + { + 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 newCartPosToSend[12] = {0.0f}; + currentCartPos.getData(newCartPosToSend); + if (REAL_ROBOT) + friInst->doCartesianImpedanceControl(newCartPosToSend,Stiff, Damp, NULL, NULL, true); + } + break; + } + + // start timer for the last sent msg + // TODO + + // check if current trajectory is over + if(currentTrajectory != NULL && currentTrajectory->getRemainingSteps() == 0) + { + std::cout << " Trajectory finished \n "; + + static int trajcetorycount = 0; + trajcetorycount +=1; + + currentTrajectory->saveToFile(std::string("trajectory/a.csv")); + // invalid trajectory skip it + delete currentTrajectory; + currentTrajectory = NULL; + } + } +} SvrHandling::~SvrHandling() { } diff --git a/lwrserv/src/main.cpp b/lwrserv/src/main.cpp index 70af2e4..c2c6c01 100644 --- a/lwrserv/src/main.cpp +++ b/lwrserv/src/main.cpp @@ -12,150 +12,6 @@ #include "Mat.h" #include "Vec.h" -void *threadRobotMovement(void *arg) -{ - // unused parameters - (void) arg; - - Trajectory* currentTrajectory = NULL; - enum MovementType currentMovementType = MovementJointBased; - - Robot::VecJoint currentJointPos(0.0f); - MatCarthesian currentCartPos(0.0f,1.0f); - - friRemote* friInst = SvrData::getInstance()->getFriRemote(); - - // get current robot position - int tries = 2; - - for (int i= 0 ; i < tries ; ++i) - { - if (REAL_ROBOT) - SvrData::getInstance()->updateMessurement(); - currentJointPos = SvrData::getInstance()->getMeasuredJointPos(); - - SvrData::getInstance()->setCommandedJointPos(currentJointPos); - - float newJointPosToSend[Robot::joints] = {0.0f}; - currentJointPos.getData(newJointPosToSend); - if (REAL_ROBOT) - friInst->doPositionControl(newJointPosToSend); - } - std::cout << "init position is " << currentJointPos << "\n"; - - while(true) - { - if (REAL_ROBOT) - SvrData::getInstance()->updateMessurement(); - - bool positionChanged = false; - // check if we have to cancel current trajectory - bool cancel = SvrData::getInstance()->getTrajectoryCancel(); - if (cancel) - { - // free the current trajectory - if ( currentTrajectory != NULL) - { - delete currentTrajectory; - currentTrajectory = NULL; - } - // signal waiting thread the cancellation - SvrData::getInstance()->trajectoryCancelDone(); - goto end; - } - - // check for new trajectory - if (currentTrajectory == NULL) - { - // get next trajectory from svrData - currentTrajectory = SvrData::getInstance()->popTrajectory(); - - // no new trajectory - if(currentTrajectory == NULL) - { - // mark that we reached the end of the trajectory - // stay on current position - goto end; - } - else - { - std::cout <<"new Trajectory with " << currentTrajectory->getSteps()<<"\n"; - } - } - - // get next commanded trajectory type - if (currentTrajectory != NULL) - { - switch (currentTrajectory->getMovementType()) - { - case MovementCartBased: - { - currentCartPos = currentTrajectory->getNextCartPos(); - positionChanged = true; - currentMovementType = MovementCartBased; - } - break; - case MovementJointBased: - { - currentJointPos = currentTrajectory->getNextJointPos(); - positionChanged = true; - currentMovementType = MovementJointBased; - } - break; - default: - { - // invalid trajectory skip it - delete currentTrajectory; - currentTrajectory = NULL; - goto end; - } - break; - } - } - -end: - switch (currentMovementType) - { - case MovementJointBased: - { - // send the new joint values - float newJointPosToSend[Robot::joints] = {0.0f}; - Robot::VecJoint newJointPosToSendRad = currentJointPos*(1./180*M_PI); - newJointPosToSendRad.getData(newJointPosToSend); - if (REAL_ROBOT) - friInst->doPositionControl(newJointPosToSend); - } - break; - case MovementCartBased: - { - 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 newCartPosToSend[12] = {0.0f}; - currentCartPos.getData(newCartPosToSend); - if (REAL_ROBOT) - friInst->doCartesianImpedanceControl(newCartPosToSend,Stiff, Damp, NULL, NULL, true); - } - break; - } - - // start timer for the last sent msg - // TODO - - // check if current trajectory is over - if(currentTrajectory != NULL && currentTrajectory->getRemainingSteps() == 0) - { - std::cout << " Trajectory finished \n "; - - static int trajcetorycount = 0; - trajcetorycount +=1; - - currentTrajectory->saveToFile(std::string("trajectory/a.csv")); - // invalid trajectory skip it - delete currentTrajectory; - currentTrajectory = NULL; - } - } -} int main() { int err = 0; @@ -163,7 +19,7 @@ int main() pthread_t fri_t; //Start the thread for the robot movement - err = pthread_create(&fri_t,NULL,&threadRobotMovement,NULL); + err = pthread_create(&fri_t,NULL,&SvrHandling::threadRobotMovement,NULL); if (err > 0 ) { std::cerr << "Failed: could not create thread\n" << std::endl;