Tracking changes from TOSCo

gps_vcones/vcones_gui.py

@@ -25,8 +25,10 @@ config_file = "vcones.yaml"
 # audio stuff
 ##############################################################################
 
+vc_path = os.path.realpath(__file__)
+
 def play_sound(file):
-  wave_obj = sa.WaveObject.from_wave_file(file)
+  wave_obj = sa.WaveObject.from_wave_file(vc_path + "/" + file)
   play_obj = wave_obj.play()
   play_obj.wait_done()
 
@@ -124,46 +126,77 @@ class VirtualCones():
     if self.num_cones == 0:
       return (0, 999.0, 0)
     
-    # end of list?
-    if self.current_cone >= self.num_cones:
-       self.current_cone = 0
-    
-    # check first node for reset
-    if self.current_cone:
-      # node from csv (each line is lat, long, distance)
-      row = self.cones[0]
+    # find node in list
+    for i in range(self.num_cones):
+      # node from csv (each line is lat, long, distance, next code#)
+      row = self.cones[i]
       cone_lat = row[0]
       cone_lon = row[1]
       cone_dist = row[2]
-      dist = roydistance(lat, lon, cone_lat, cone_lon)
-      if dist < rng:
-        self.current_cone = 0
-      
-    # node from csv (each line is lat, long, distance, next code#)
-    row = self.cones[self.current_cone]
-    cone_lat = row[0]
-    cone_lon = row[1]
-    cone_dist = row[2]
-          
-    # next cone
-    cone_next = self.current_cone + 1
-    try:
-      cone_next = int(row[3])
-    except:
-      pass
-    
+
       # distance between vehicle and cone
-    self.distance = roydistance(lat, lon, cone_lat, cone_lon)
-                    
-    # check distance trigger
-    if self.distance < rng:
-#      out = "Waypoint %s:%d reached: %f, %f, %f, %d" % (self.filename, self.current_cone, lat, lon,  heading, speed)
-#      print(out)
-      play_sound("alert.wav")
-      self.current_cone = cone_next
-      trig = 1
+      self.distance = roydistance(lat, lon, cone_lat, cone_lon)
+      
+      # check distance trigger
+      if self.distance < rng:
+#        out = "Waypoint %s:%d reached: %f, %f, %f, %d" % (self.filename, self.current_cone, lat, lon,  heading, speed)
+#        print(out)
+        self.current_cone = i
+        trig = 1
+        if self.buzz_no < self.max_buzz:
+          play_sound("alert.wav")
+          self.buzz_no = self.buzz_no + 1
+        break
+      else:
+        self.buzz_no = 0
     
     return (self.current_cone, self.distance, trig)
+  
+#  def check_list(self, lat, lon, heading, speed, rng):
+#    trig = 0
+#    if self.num_cones == 0:
+#      return (0, 999.0, 0)
+    
+#    # end of list?
+#    if self.current_cone >= self.num_cones:
+#       self.current_cone = 0
+    
+    # check first node for reset
+#    if self.current_cone:
+      # node from csv (each line is lat, long, distance)
+#      row = self.cones[0]
+#      cone_lat = row[0]
+#      cone_lon = row[1]
+#      cone_dist = row[2]
+#      dist = roydistance(lat, lon, cone_lat, cone_lon)
+#      if dist < rng:
+#        self.current_cone = 0
+      
+    # node from csv (each line is lat, long, distance, next code#)
+#    row = self.cones[self.current_cone]
+#    cone_lat = row[0]
+#    cone_lon = row[1]
+#    cone_dist = row[2]
+          
+    # next cone
+#    cone_next = self.current_cone + 1
+#    try:
+#      cone_next = int(row[3])
+#    except:
+#      pass
+    
+      # distance between vehicle and cone
+#    self.distance = roydistance(lat, lon, cone_lat, cone_lon)
+                    
+    # check distance trigger
+#    if self.distance < rng:
+#      out = "Waypoint %s:%d reached: %f, %f, %f, %d" % (self.filename, self.current_cone, lat, lon,  heading, speed)
+#      print(out)
+#      play_sound("alert.wav")
+#      self.current_cone = cone_next
+#      trig = 1
+    
+#    return (self.current_cone, self.distance, trig)
 
 ##############################################################################
 # thread