fix svrData retvals

lwrserv/SvrData.cpp

@@ -12,11 +12,6 @@ SvrData::SvrData(void)
     velocity = 20;
     accelaration = 10;
     robot.joints = LBR_MNJ;
-    
-    robot.max.velocity = new float[robot.joints];
-    robot.max.accelaration = new float[robot.joints];
-    robot.max.torque = new float[robot.joints];
-    robot.max.range = new float[robot.joints];
 
     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};
@@ -39,22 +34,6 @@ SvrData::SvrData(void)
 
 SvrData::~SvrData()
 {
-    if (robot.max.velocity != 0)
-        delete[] robot.max.velocity;
-
-    if (robot.max.accelaration != 0)
-        delete[] robot.max.accelaration;
-
-    if (robot.max.torque != 0)
-        delete[] robot.max.torque;
-
-    if (robot.max.velocity != 0)
-        delete[] robot.max.range;
-
-    robot.max.velocity = 0;
-    robot.max.accelaration = 0;
-    robot.max.torque = 0;
-    robot.max.range = 0;
     pthread_mutex_destroy(&dataLock);
 }
 
@@ -89,6 +68,7 @@ int SvrData::getMessuredJointPos(float* data, size_t size)
     pthread_mutex_lock(&dataLock);
     memcpy(data,messured.jointPos,size);
     pthread_mutex_unlock(&dataLock);
+    return 0;
 }
 
 int SvrData::getMessuredCartPos (float* data, size_t size)
@@ -101,6 +81,7 @@ int SvrData::getMessuredCartPos (float* data, size_t size)
     pthread_mutex_lock(&dataLock);
     memcpy(data,messured.cartPos,size);
     pthread_mutex_unlock(&dataLock);
+    return 0 ;
 }
 
 int SvrData::getMessuredJacobian(float* data, size_t size)
@@ -113,6 +94,7 @@ int SvrData::getMessuredJacobian(float* data, size_t size)
     pthread_mutex_lock(&dataLock);
     memcpy(data,messured.jacobian,size);
     pthread_mutex_unlock(&dataLock);
+    return 0;
 }
 
 int SvrData::getMessuredForceTorque(float* data, size_t size)
@@ -125,6 +107,7 @@ int SvrData::getMessuredForceTorque(float* data, size_t size)
     pthread_mutex_lock(&dataLock);
     memcpy(data,messured.forceAndTorque,size);
     pthread_mutex_unlock(&dataLock);
+    return 0;
 }
 
 int SvrData::getCommandedJointPos(float* data, size_t size)
@@ -137,6 +120,8 @@ int SvrData::getCommandedJointPos(float* data, size_t size)
     pthread_mutex_lock(&dataLock);
     memcpy(data,commanded.jointPos,size);
     pthread_mutex_unlock(&dataLock);
+    
+    return 0;
 }
 
 int SvrData::getCommandedCartPos (float* data, size_t size)
@@ -149,6 +134,8 @@ int SvrData::getCommandedCartPos (float* data, size_t size)
     pthread_mutex_lock(&dataLock);
     memcpy(data,commanded.cartPos,size);
     pthread_mutex_unlock(&dataLock);
+
+    return 0;
 }
 
 float SvrData::getMaxTorque(unsigned int pos)
@@ -160,6 +147,7 @@ float SvrData::getMaxTorque(unsigned int pos)
     pthread_mutex_lock(&dataLock);
     retval = robot.max.torque[pos-1];
     pthread_mutex_unlock(&dataLock);
+    return retval;
 }
 
 int   SvrData::getMaxTorque(float* data, size_t size)
@@ -172,9 +160,10 @@ int   SvrData::getMaxTorque(float* data, size_t size)
     pthread_mutex_lock(&dataLock);
     memcpy(data,robot.max.torque,size);
     pthread_mutex_unlock(&dataLock);
+    return 0;
 }
 
-float SvrData::getMaxAccelaration(unsigned int pos)
+float SvrData::getMaxAcceleration(unsigned int pos)
 {
     float retval = -1.0f;
     if (pos >= robot.joints)
@@ -183,8 +172,9 @@ float SvrData::getMaxAccelaration(unsigned int pos)
     pthread_mutex_lock(&dataLock);
     retval = robot.max.accelaration[pos-1];
     pthread_mutex_unlock(&dataLock);
+    return retval;
 }
-int   SvrData::getMaxAccelaration(float* data, size_t size)
+int   SvrData::getMaxAcceleration(float* data, size_t size)
 {
     if (data == NULL)
         return -EINVAL;
@@ -194,6 +184,15 @@ int   SvrData::getMaxAccelaration(float* data, size_t size)
     pthread_mutex_lock(&dataLock);
     memcpy(data,robot.max.accelaration,size);
     pthread_mutex_unlock(&dataLock);
+    return 0;
+}
+Vec<float,LBR_MNJ>   SvrData::getMaxAcceleration()
+{   
+    Vec<float,LBR_MNJ> buff;
+    pthread_mutex_lock(&dataLock);
+    buff = robot.max.accelaration;
+    pthread_mutex_unlock(&dataLock);
+    return buff;
 }
 
 float SvrData::getMaxRange(unsigned int pos)
@@ -205,6 +204,7 @@ float SvrData::getMaxRange(unsigned int pos)
     pthread_mutex_lock(&dataLock);
     retval = robot.max.range[pos-1];
     pthread_mutex_unlock(&dataLock);
+    return retval;
 }
 
 int   SvrData::getMaxRange(float* data, size_t size)
@@ -217,6 +217,7 @@ int   SvrData::getMaxRange(float* data, size_t size)
     pthread_mutex_lock(&dataLock);
     memcpy(data,robot.max.range,size);
     pthread_mutex_unlock(&dataLock);
+    return 0;
 }
 
 unsigned int SvrData::getJoints()