Browse Source

Sound support

master
Graham Northup 5 years ago
parent
commit
2576556c32
Signed by: grissess GPG Key ID: 5D000E6F539376FB
  1. 12
      cli.py
  2. 1
      packet.py
  3. 12
      tbot_udp.py
  4. 27
      teleop.py

12
cli.py

@ -59,6 +59,18 @@ class TBot(cmd.Cmd):
pkt.write_double(float(rest))
self.socket.sendto(bytes(pkt), self.addr)
def do_snd(self, rest):
'snd <sound, [0,6]>'
if not self.addr:
print('Not connected')
return
if not rest:
rest = '0'
pkt = Packet()
pkt.write_ubyte(CMD.SOUND)
pkt.write_ubyte(int(rest))
self.socket.sendto(bytes(pkt), self.addr)
def do_EOF(self, rest):
print('Goodbye.')
if self.addr:

1
packet.py

@ -45,3 +45,4 @@ class Packet(object):
class CMD:
KEEPALIVE = 0
MOTION = 1
SOUND = 2

12
tbot_udp.py

@ -5,7 +5,7 @@ from cStringIO import StringIO
import rospy
from geometry_msgs.msg import Twist
from kobuki_msgs.msg import BumperEvent
from kobuki_msgs.msg import BumperEvent, Sound
##### Configuration #####
@ -70,7 +70,7 @@ class Packet(object):
class CMD:
KEEPALIVE = 0
MOTION = 1
SOUND = 2
class Processor(object):
dispatch = {}
@ -146,11 +146,19 @@ def proc_cmd_motion(self, pkt):
self.motion.angular.z = pkt.read_double()
print self.src, ':', 'motion', self.motion.linear.x, self.motion.angular.z
sound_node = None
@Processor._dispatch(CMD.SOUND)
def proc_cmd_sound(self, pkt):
snd = pkt.read_ubyte()
print 'Sound:', snd
sound_node.publish(Sound(snd))
if __name__ == '__main__':
proc = Processor((LISTEN_ADDR, LISTEN_PORT))
rospy.init_node('tbot', anonymous=False)
motion_node = rospy.Publisher('mobile_base/commands/velocity', Twist, queue_size=10)
rospy.Subscriber('mobile_base/events/bumper', BumperEvent, proc.on_bump)
sound_node = rospy.Publisher('mobile_base/commands/sound', Sound, queue_size=10)
rate = rospy.Rate(CMD_RATE)
rospy.on_shutdown(lambda: motion_node.publish(Twist()))
while not rospy.is_shutdown():

27
teleop.py

@ -1,6 +1,7 @@
import socket
import pygame
import sys
import time
from packet import Packet, CMD
@ -21,8 +22,8 @@ PARMS = {
0: 'LIN_V',
1: 'ANG_V',
}
UP = 5
DOWN = 4
UP = 5
TIMER_KA = pygame.USEREVENT
KA_PKT = Packet()
@ -33,12 +34,21 @@ pygame.time.set_timer(TIMER_KA, 100)
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
addr = (sys.argv[1], int(sys.argv[2]))
lasthat = None
vmap = [
[7, 0, 1],
[6, -1, 2],
[5, 4, 3],
]
clock = pygame.time.Clock()
while True:
for ev in pygame.event.get():
if ev.type == pygame.KEYDOWN and ev.key == pygame.K_ESCAPE:
exit()
if ev.type == pygame.JOYBUTTONDOWN and ev.joy == joy.get_id():
print('bd:', ev.button)
if ev.type == pygame.JOYHATMOTION and ev.joy == joy.get_id():
lasthat = time.time()
if ev.type == TIMER_KA:
sock.sendto(bytes(KA_PKT), addr)
if any(joy.get_button(i) for i in (UP, DOWN)):
@ -50,6 +60,16 @@ while True:
cnt += 1
if not cnt:
print('WARN: no parms modified, try pressing another button')
if lasthat is not None and time.time() - lasthat > 0.05:
hx, hy = joy.get_hat(0)
val = vmap[1 - hy][1 + hx]
print('snd', val)
if val >= 0 and val <= 6:
pkt = Packet()
pkt.write_ubyte(CMD.SOUND)
pkt.write_ubyte(val)
sock.sendto(bytes(pkt), addr)
lasthat = None
lv, av = joy.get_axis(LIN_AX), joy.get_axis(ANG_AX)
pkt = Packet()
pkt.write_ubyte(CMD.MOTION)
@ -73,3 +93,4 @@ while True:
bottom -= surf.get_height()
pygame.display.flip()
clock.tick(30)
Loading…
Cancel
Save