added battery status

robotika · May 13, 2015 · b2fbb84 · b2fbb84
1 parent 346cf33
commit b2fbb84
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 1 deletion.
diff --git a/ros/go.py b/ros/go.py
@@ -92,7 +92,7 @@ def go( metalog, assertWrite, ipPair ):
             prev = minDist[0]/1000.0
             turnDiff = 160-minDist[1] # only flip
             obstacleDir = -math.radians(turnDiff)/4.0 # TODO calibration via FOV
-            print prev, turnDiff
+            print prev, turnDiff, robot.power
 
         if prev is None or prev < safeDist:
             robot.setSpeedPxPa( 0, 0 )

diff --git a/ros/huskyros.py b/ros/huskyros.py
@@ -35,6 +35,7 @@ def __init__( self, filename=None, replay=False, assertWrite=True, ipPair=None )
         self.angularSpeed = 0.0
         self.enc = None # unknown
         self.time = None
+        self.power = None
         self.imu = None
         self.mag = None
         self.azimuth = None
@@ -74,6 +75,9 @@ def update( self ):
                 self.time = data[0]
                 self.enc = data[1][0], data[2][0] # only distance traveled
 
+            if topic == '/husky/data/power_status':
+                self.power = data
+
     def setSpeedPxPa( self, speed, angularSpeed ):
         self.speed = speed
         self.angularSpeed = angularSpeed