Changes in velocity package and sending velocity

Interchange sending velocity command in gr_Command

skills/skill_node.py

@@ -15,8 +15,8 @@ def send_command(pub, team, bot_id, v_x, v_y, v_w, kick_power, dribble, chip_pow
 	# Set the command to each bot
 	gr_command.id          = bot_id
 	gr_command.wheelsspeed = 0
-	gr_command.veltangent  = -v_y/1000.0
-	gr_command.velnormal   = +v_x/1000.0
+	gr_command.veltangent  = v_x/1000.0
+	gr_command.velnormal   = v_y/1000.0
 	gr_command.velangular  = v_w
 	gr_command.kickspeedx  = kick_power
 	gr_command.kickspeedz  = chip_power

velocity/profiler.py

@@ -102,9 +102,9 @@ def GetExpectedPositionIndex(self):
 	##
 	## @brief      Total length of path
 	##
-	def GetPathLength(self):
+	def GetPathLength(self, startIndex=1):
 		length = 0
-		for i in xrange(1,len(self.path)):
+		for i in xrange(startIndex,len(self.path)):
 			length += self.path[i].dist(self.path[i-1])
 		return length
 

velocity/run.py

@@ -54,9 +54,10 @@ def Get_Vel(start, t, kub_id, target, homePos_, awayPos_,avoid_ball=False):
         FIRST_CALL = 0
 
     if distance < 1.5*BOT_BALL_THRESH:
-        return [0,0,0,0]
+        return [0,0,0,0,0]
     # print("ex = ",expectedTraverseTime) 
     # print("t = ",t," start = ",start)
+    remainingDistance = 0
     if (t - start< expectedTraverseTime):
         if v.trapezoid(t - start,curPos):
             index = v.GetExpectedPositionIndex()
@@ -65,6 +66,7 @@ def Get_Vel(start, t, kub_id, target, homePos_, awayPos_,avoid_ball=False):
                 vX,vY = 0,0
 
             else:
+                remainingDistance = v.GetPathLength(startIndex=index)
                 vX,vY,eX,eY = v.sendVelocity(v.getVelocity(),v.motionAngle[index],index)
 
         else:
@@ -109,15 +111,15 @@ def Get_Vel(start, t, kub_id, target, homePos_, awayPos_,avoid_ball=False):
             startPt.x = homePos[kubid].x
             startPt.y = homePos[kubid].y
             findPath(startPt,target, avoid_ball)
-            return [0,0,0, REPLAN]  
+            return [0,0,0, REPLAN,0]  
     else:
         errorInfo.errorX = eX
         errorInfo.errorY = eY
         vX,vY = pid(vX,vY,errorInfo,pso)
         botAngle = homePos[kubid].theta
         vXBot = vX*cos(botAngle) + vY*sin(botAngle)
         vYBot = -vX*sin(botAngle) + vY*cos(botAngle)
-        return [vXBot, vYBot, 0, REPLAN]            
+        return [vXBot, vYBot, 0, REPLAN,remainingDistance]            
     # print("end getVelocity")    
 
 def shouldReplan():