telemetry for balboa

BalancerRPi @ d1ab5385

-Subproject commit b02cf4d69a682e80d7bf3a6fdf0da877dceaacbd
+Subproject commit d1ab5385d38f8415905094896e0b6fffdadec82a

ros_ws/src/balboa_lowlevel/scripts/balboa_interface.py

@@ -2,6 +2,7 @@
 from __future__ import print_function
 import rospy
 from geometry_msgs.msg import Twist
+from std_msgs.msg import Int32
 import serial
 
 
@@ -9,7 +10,12 @@ class BalboaInterface:
     def __init__(self, port="/dev/ttyACM0"):
         rospy.init_node('balboa_interface', anonymous=True)
         rospy.Subscriber("key_vel", Twist, self.callback)
-        self.ser = serial.Serial(port, timeout=1)
+        self.pub_left_encoder = rospy.Publisher('left_encoder', Int32, queue_size=10)
+        self.pub_right_encoder = rospy.Publisher('right_encoder', Int32, queue_size=10)
+        self.pub_velocity = rospy.Publisher('velocity', Int32, queue_size=10)
+        self.pub_angle = rospy.Publisher('angle', Int32, queue_size=10)
+        self.pub_motor_speed = rospy.Publisher('motor_speed', Int32, queue_size=10)
+        self.ser = serial.Serial(port, 115200, timeout=1)
 
     def callback(self, msg):
         self.ser.write(b"{} {}\n".format(
@@ -17,11 +23,24 @@ class BalboaInterface:
             int(msg.linear.x * 100)
         ))
 
+    def parse_line(self, line):
+        values = line.split()
+        t = values[0][:-1]
+        values = values[1:]
+        if t == 'O':
+            self.pub_left_encoder.publish(int(values[2]))
+            self.pub_right_encoder.publish(int(values[3]))
+            self.pub_velocity.publish(int(values[0]))
+        elif t == 'DT':
+            self.pub_angle.publish(int(values[0])/10)
+            self.pub_motor_speed.publish(int(values[1]))
+
     def run(self):
         rate = rospy.Rate(10)
         while not rospy.is_shutdown():
             while self.ser.in_waiting > 0:
                 line = self.ser.readline()
+                self.parse_line(line)
                 print(line, end='')
             rate.sleep()