Commit a12e289c authored by Barciś, Agata's avatar Barciś, Agata
Browse files

telemetry for balboa

parent f7e92ceb
BalancerRPi @ d1ab5385
Subproject commit b02cf4d69a682e80d7bf3a6fdf0da877dceaacbd
Subproject commit d1ab5385d38f8415905094896e0b6fffdadec82a
......@@ -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()
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment