Merge pull request #11 from kaka012/add-sonar-perceptor

Implement the sonar perceptor in spark_agent.py

Author: xuyuan

software_installation/spark_agent.py

@@ -6,7 +6,6 @@
 from threading import Thread
 from math import pi, atan2, asin, cos, sin
 from sexpr import str2sexpr
-import numpy as np
 
 DEG_TO_RAD = pi / 180
 
@@ -19,6 +18,7 @@
 GAME_STATE_PERCEPTOR = "GS"
 GPS_PERCEPTOR = "GPS"
 BAT_PERCEPTOR = "BAT"
+SONAR_PERCEPTOR = "US"
 
 VISION_PERCEPTOR = "See"
 VISION_PERCEPTOR_TRUE_BALL = "ballpos"
@@ -132,6 +132,7 @@ def __init__(self):
         self.game_state = GameState()
         self.gps = {}
         self.imu = [0, 0] # [AngleX, AngleY]
+        self.us = {}
 
     def update(self, sexp):
         for s in sexp:
@@ -164,6 +165,8 @@ def update(self, sexp):
                 self.gps[s[1][1]] = [float(v) for v in s[2][1:]]
             elif name == BAT_PERCEPTOR:
                 self.bat = float(s[1])
+            elif name == SONAR_PERCEPTOR:
+                self.us[s[1]] = [float(dist) for dist in s[2]]
             else:
                 raise RuntimeError('unknown perception: ' + str(s))