move thread to svrHandling

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);
     
 };
 

lwrserv/src/SvrData.cpp

@@ -450,7 +450,7 @@ class Trajectory<Robot::joints>* SvrData::createTrajectory(Robot::VecJoint newJo
     //set new target for next trajectory
     Robot::VecJoint prevJointPos = getCommandedJointPos();
     class Trajectory<Robot::joints>* newTrajectory;
-    
+
     float sampleTimeMs = getSampleTimeMs();
     Robot::VecJoint maxJointVelocity     =  getRobotVelocity();
     Robot::VecJoint maxJointAcceleration =  getRobotAcceleration();
@@ -463,7 +463,9 @@ class Trajectory<Robot::joints>* SvrData::createTrajectory(Robot::VecJoint newJo
 }
 class Trajectory<Robot::joints>* SvrData::createTrajectory(MatCarthesian newJointPos)
 {
-
+    (void) newJointPos;
+    // unimplemented
+    return NULL;
 }
 Robot::VecJoint SvrData::getMeasuredJointPos()
 {

lwrserv/src/SvrHandling.cpp

@@ -105,6 +105,150 @@ SvrHandling::SvrHandling()
     commandCount = i;
 }
 
+void * SvrHandling::threadRobotMovement(void *arg)
+{
+    // unused parameters 
+    (void) arg;
+
+    Trajectory<Robot::joints>* 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()
 {
 }

lwrserv/src/main.cpp

@@ -12,150 +12,6 @@
 #include "Mat.h"
 #include "Vec.h"
 
-void *threadRobotMovement(void *arg)
-{
-    // unused parameters 
-    (void) arg;
-
-    Trajectory<Robot::joints>* 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;