Update control.py

Author: abdulkadrtr

nav_controller/nav_controller/control.py

@@ -106,10 +106,11 @@ def bspline_planning(x, y, sn):
 
     return rx, ry
 
-def pure_pursuit(current_x, current_y, current_heading, path,lookahead_distance,index):
+def pure_pursuit(current_x, current_y, current_heading, path):
+    global lookahead_distance
     closest_point = None
     v = speed
-    for i in range(index,len(path)):
+    for i in range(len(path)):
         x = path[i][0]
         y = path[i][1]
         distance = math.hypot(current_x - x, current_y - y)
@@ -132,7 +133,8 @@ def pure_pursuit(current_x, current_y, current_heading, path,lookahead_distance,
         sign = 1 if desired_steering_angle > 0 else -1
         desired_steering_angle = sign * math.pi/4
         v = 0.0
-    return v,desired_steering_angle,index
+    path = path[index-2:]
+    return v,desired_steering_angle,path
 
 def costmap(data,width,height,resolution):
     data = np.array(data).reshape(height,width)
@@ -211,13 +213,12 @@ def listener_callback(self,msg):
             self.path = bspline_planning(path,len(path)*5) #bspline ile düzeltme
             print("Robot Konumu: ",self.x,self.y)
             print("Hedefe ilerleniyor...")
-            self.i = 0
             self.flag = 2
 
     def timer_callback(self):
         if self.flag == 2:
             twist = Twist()
-            twist.linear.x , twist.angular.z,self.i = pure_pursuit(self.x,self.y,self.yaw,self.path,lookahead_distance,self.i)
+            twist.linear.x , twist.angular.z, self.path = pure_pursuit(self.x,self.y,self.yaw,self.path)
             if(abs(self.x - self.path[-1][0]) < 0.05 and abs(self.y - self.path[-1][1])< 0.05):
                 twist.linear.x = 0.0
                 twist.angular.z = 0.0
@@ -242,4 +243,3 @@ def main(args=None):
 
 if __name__ == '__main__':
     main()
-