Browse Source

Initial commit

master
Graham Northup 5 years ago
commit
fac959d160
Signed by: grissess GPG Key ID: 5D000E6F539376FB
  1. 1
      .gitignore
  2. 77
      cli.py
  3. 47
      packet.py
  4. 159
      tbot_udp.py
  5. 75
      teleop.py
  6. 194
      twibot.py

1
.gitignore

@ -0,0 +1 @@
__pycache__

77
cli.py

@ -0,0 +1,77 @@
import socket
import cmd
import threading
import time
from packet import Packet, CMD
class KAThread(threading.Thread):
def run(self):
tbot = self._args[0]
pkt = Packet()
pkt.write_ubyte(CMD.KEEPALIVE)
while True:
if tbot.addr:
try:
time.sleep(0.1)
tbot.socket.sendto(bytes(pkt), tbot.addr)
except Exception:
pass
class TBot(cmd.Cmd):
socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
addr = None
def do_connect(self, rest):
'connect [<host>[:<port>]]'
if rest:
host, _, port = rest.partition(':')
if not port:
port = '13676'
self.addr = (host, int(port))
else:
self.addr = None
print('Remote set to', self.addr)
def do_fw(self, rest):
'fw <velocity:m/s>'
if not self.addr:
print('Not connected')
return
if not rest:
rest = '0'
pkt = Packet()
pkt.write_ubyte(CMD.MOTION)
pkt.write_double(float(rest))
pkt.write_double(0.0)
self.socket.sendto(bytes(pkt), self.addr)
def do_lt(self, rest):
'lt <velocity:rad/s>'
if not self.addr:
print('Not connected')
return
if not rest:
rest = '0'
pkt = Packet()
pkt.write_ubyte(CMD.MOTION)
pkt.write_double(0.0)
pkt.write_double(float(rest))
self.socket.sendto(bytes(pkt), self.addr)
def do_EOF(self, rest):
print('Goodbye.')
if self.addr:
pkt = Packet()
pkt.write_ubyte(CMD.MOTION)
pkt.write_double(0.0)
pkt.write_double(0.0)
self.socket.sendto(bytes(pkt), self.addr)
exit()
if __name__ == '__main__':
tbot = TBot()
tthr = KAThread(args=(tbot,))
tthr.setDaemon(True)
tthr.start()
tbot.cmdloop()

47
packet.py

@ -0,0 +1,47 @@
import struct
import socket
from io import BytesIO as StringIO
class Packet(object):
def __init__(self, ival = None):
if ival is not None:
self.buffer = StringIO(ival)
else:
self.buffer = StringIO()
# Staticmethod
def _reader(fmt):
return lambda self: struct.unpack(fmt, self.buffer.read(struct.calcsize(fmt)))[0]
# Staticmethod
def _writer(fmt):
return lambda self, obj: self.buffer.write(struct.pack(fmt, obj))
read_byte = _reader('!b')
read_ubyte = _reader('!B')
read_short = _reader('!h')
read_ushort = _reader('!H')
read_int = _reader('!i')
read_uint = _reader('!I')
read_long = _reader('!l')
read_ulong = _reader('!L')
read_float = _reader('!f')
read_double = _reader('!d')
write_byte = _writer('!b')
write_ubyte = _writer('!B')
write_short = _writer('!h')
write_ushort = _writer('!H')
write_int = _writer('!i')
write_uint = _writer('!I')
write_long = _writer('!l')
write_ulong = _writer('!L')
write_float = _writer('!f')
write_double = _writer('!d')
def __bytes__(self):
return self.buffer.getvalue()
class CMD:
KEEPALIVE = 0
MOTION = 1

159
tbot_udp.py

@ -0,0 +1,159 @@
import socket
import struct
import time
from cStringIO import StringIO
import rospy
from geometry_msgs.msg import Twist
from kobuki_msgs.msg import BumperEvent
##### Configuration #####
# Address to listen to (empty indicates all interfaces)
LISTEN_ADDR=''
LISTEN_PORT=13676
# Cut power if we lose keepalives
TERM_ON_KA=True
KA_TIMEOUT = 3 # seconds
# Rate with which to publish commands (also controls responsiveness)
CMD_RATE = 10
# Don't set linear velocity if the bumper is detected
DONT_BUMP=True
BUMP_REV_SPEED = 0.05 # This MUST be positive
BUMP_REV_TIME = 1.5
##### Protocol #####
class Packet(object):
def __init__(self, ival = None):
if ival is not None:
self.buffer = StringIO(ival)
else:
self.buffer = StringIO()
# Staticmethod
def _reader(fmt):
return lambda self: struct.unpack(fmt, self.buffer.read(struct.calcsize(fmt)))[0]
# Staticmethod
def _writer(fmt):
return lambda self, obj: self.buffer.write(struct.pack(fmt, obj))
read_byte = _reader('!b')
read_ubyte = _reader('!B')
read_short = _reader('!h')
read_ushort = _reader('!H')
read_int = _reader('!i')
read_uint = _reader('!I')
read_long = _reader('!l')
read_ulong = _reader('!L')
read_float = _reader('!f')
read_double = _reader('!d')
write_byte = _writer('!b')
write_ubyte = _writer('!B')
write_short = _writer('!h')
write_ushort = _writer('!H')
write_int = _writer('!i')
write_uint = _writer('!I')
write_long = _writer('!l')
write_ulong = _writer('!L')
write_float = _writer('!f')
write_double = _writer('!d')
def __str__(self):
return self.buffer.getvalue()
class CMD:
KEEPALIVE = 0
MOTION = 1
class Processor(object):
dispatch = {}
motion = Twist()
lastka = None
impact = [False]*3
backingsince = None
def __init__(self, addr):
self.socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
self.socket.bind(addr)
self.socket.setblocking(False)
print 'Listener ready on', addr
def tick(self):
try:
while True:
data, src = self.socket.recvfrom(4096)
self.src = src
self.process_pkt(Packet(data))
except socket.error:
pass
if self.lastka is not None and time.time() > self.lastka + KA_TIMEOUT:
print 'KA timeout'
self.lastka = None
if TERM_ON_KA:
print '(killing motors)'
self.motion.linear.x = 0
self.motion.angular.z = 0
def process_pkt(self, pkt):
self.cmd = pkt.read_ubyte()
self.dispatch.get(self.cmd, self.no_cmd)(self, pkt)
def no_cmd(self, _, pkt):
print 'Warning: unknown command:', self.cmd
def on_bump(self, status):
self.impact[status.bumper] = bool(status.state)
def get_motion(self):
t = Twist()
buf = StringIO()
self.motion.serialize(buf)
t.deserialize(buf.getvalue())
if DONT_BUMP:
if any(self.impact):
self.backingsince = time.time()
self.motion.linear.x = 0
if self.backingsince is not None:
if time.time() < self.backingsince + BUMP_REV_TIME:
t.linear.x = -BUMP_REV_SPEED
else:
self.backingsince = None
return t
@classmethod
def _dispatch(cls, cmd):
def _inner(f):
cls.dispatch[cmd] = f
return f
return _inner
@Processor._dispatch(CMD.KEEPALIVE)
def proc_cmd_keepalive(self, pkt):
if self.lastka is None:
print 'New KA from', self.src
self.lastka = time.time()
@Processor._dispatch(CMD.MOTION)
def proc_cmd_motion(self, pkt):
self.motion.linear.x = pkt.read_double()
self.motion.angular.z = pkt.read_double()
print self.src, ':', 'motion', self.motion.linear.x, self.motion.angular.z
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)
rate = rospy.Rate(CMD_RATE)
rospy.on_shutdown(lambda: motion_node.publish(Twist()))
while not rospy.is_shutdown():
proc.tick()
motion_node.publish(proc.get_motion())
rate.sleep()

75
teleop.py

@ -0,0 +1,75 @@
import socket
import pygame
import sys
from packet import Packet, CMD
pygame.init()
disp = pygame.display.set_mode((640, 480))
font = pygame.font.SysFont(pygame.font.get_default_font(), 24)
joys = [pygame.joystick.Joystick(i) for i in range(pygame.joystick.get_count())]
for joy in joys:
joy.init()
LIN_AX = 1
ANG_AX = 0
LIN_V = 0.2
ANG_V = 1.0
PARMS = {
0: 'LIN_V',
1: 'ANG_V',
}
UP = 5
DOWN = 4
TIMER_KA = pygame.USEREVENT
KA_PKT = Packet()
KA_PKT.write_ubyte(CMD.KEEPALIVE)
pygame.time.set_timer(TIMER_KA, 100)
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
addr = (sys.argv[1], int(sys.argv[2]))
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 == TIMER_KA:
sock.sendto(bytes(KA_PKT), addr)
if any(joy.get_button(i) for i in (UP, DOWN)):
cnt = 0
for but, var in PARMS.items():
if joy.get_button(but):
globals()[var] += (0.1 if joy.get_button(UP) else -0.1)
print(var, ':', globals()[var])
cnt += 1
if not cnt:
print('WARN: no parms modified, try pressing another button')
lv, av = joy.get_axis(LIN_AX), joy.get_axis(ANG_AX)
pkt = Packet()
pkt.write_ubyte(CMD.MOTION)
pkt.write_double(-LIN_V * lv)
pkt.write_double(-ANG_V * av)
sock.sendto(bytes(pkt), addr)
w, h = disp.get_size()
r = int(min(w / 2, h / 2))
disp.fill((0, 0, 0))
pygame.draw.circle(disp, (0, 0, 255), (int(w / 2), int(h / 2)), r, 1)
pygame.draw.rect(disp, (127, 0, 255), (int(w / 2 - r), int(h / 2 - r), 2*r, 2*r), 1)
pygame.draw.circle(disp, (255, 255, 255), (int(w / 2 + r * av), int(h / 2 + r * lv)), 5)
bottom = h
for but, var in PARMS.items():
if joy.get_button(but):
val = globals()[var]
surf = font.render('%s: %0.1f'%(var, val), True, (255, 255, 255))
disp.fill((0, 0, 255), (0, bottom - surf.get_height(), int(w * val / 5), surf.get_height()))
disp.blit(surf, (int(w / 2 - surf.get_width() / 2), bottom - surf.get_height()))
bottom -= surf.get_height()
pygame.display.flip()

194
twibot.py

@ -0,0 +1,194 @@
#!/usr/bin/env python3
import pydle
import socket
import cmd
import queue
import threading
import time
import traceback
from packet import Packet, CMD
VOTE_TIME=0.5
CHANNEL='#turtle_donatello'
FB_SPEED = 0.2
LR_SPEED = 1.0
TURTLEBOT_ADDR = ('127.0.0.1', 13676)
socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
cmd_queue = queue.Queue(8)
stop = Packet()
stop.write_ubyte(CMD.MOTION)
stop.write_double(0.0)
stop.write_double(0.0)
class KAThread(threading.Thread):
def run(self):
pkt = Packet()
pkt.write_ubyte(CMD.KEEPALIVE)
while True:
socket.sendto(bytes(pkt), TURTLEBOT_ADDR)
time.sleep(0.25)
kathr = KAThread()
kathr.setDaemon(True)
kathr.start()
def try_message(target, msg):
return
print('SENT', target, ':', msg)
try:
tirc.message(target, msg)
except Exception:
traceback.print_exc()
class CmdThread(threading.Thread):
def run(self):
while True:
try_message(CHANNEL, '--- {}s voting round begins ---'.format(VOTE_TIME))
time.sleep(VOTE_TIME)
print('--- Voting Round ---')
major = tbcmds.get_majority()
if not major:
print('No commands.')
continue
print('Winner: {}'.format(tbcmds.pretty_cmd(major)))
try_message(CHANNEL, 'Voted majority: {}'.format(tbcmds.pretty_cmd(major)))
vfw, vlr, tm = major
print('Starting motion:', vfw, vlr, tm)
pkt = Packet()
pkt.write_ubyte(CMD.MOTION)
pkt.write_double(vfw)
pkt.write_double(vlr)
socket.sendto(bytes(pkt), TURTLEBOT_ADDR)
time.sleep(tm)
print('Stopping motion:', vfw, vlr, tm)
socket.sendto(bytes(stop), TURTLEBOT_ADDR)
cmdthr = CmdThread()
cmdthr.setDaemon(True)
cmdthr.start()
class TBotCmds(cmd.Cmd):
votes = {}
votelock = threading.Lock()
has_cmd = False
def issue_cmd(self, cmd):
with self.votelock:
for k, v in self.votes.items():
v.discard(self.context)
self.votes.setdefault(cmd, set()).add(self.context)
def get_majority(self):
with self.votelock:
if not self.votes:
return None
all_votes = sorted(self.votes.items(), key=lambda pair: len(pair[1]), reverse = True)
self.votes.clear()
return all_votes[0][0]
def repr_votes(self):
with self.votelock:
if not self.votes:
return '(None)'
return ', '.join('{} for {}'.format(len(v), self.pretty_cmd(k)) for k, v in self.votes.items())
def pretty_cmd(self, cmd):
fb_name = 'In Place'
if cmd[0] > 0:
fb_name = 'Forward {}m/s'.format(cmd[0])
if cmd[0] < 0:
fb_name = 'Backward {}m/s'.format(cmd[0])
lr_name = 'Straight'
if cmd[1] > 0:
lr_name = 'Left {}rad/s'.format(cmd[1])
if cmd[1] < 0:
lr_name = 'Right {}rad/s'.format(cmd[1])
return '{} {} for {}s'.format(fb_name, lr_name, cmd[2])
@staticmethod
def clamp_time(ts):
if not ts:
ts = '1'
t = float(ts)
return max(min(1000000, t), 0.5)
def do_forward(self, rest):
t = self.clamp_time(rest)
self.issue_cmd((FB_SPEED, 0.0, t))
self.has_cmd = True
do_fw = do_forward
do_f = do_forward
do_w = do_forward
#def do_backward(self, rest):
# t = self.clamp_time(rest)
# self.issue_cmd((-FB_SPEED, 0.0, t))
# self.has_cmd = True
#do_back = do_backward
#do_bw = do_backward
#do_b = do_backward
def do_stay(self, rest):
t = self.clamp_time(rest)
self.issue_cmd((0.0, 0.0, t))
self.has_cmd = True
do_stop = do_stay
do_s = do_stay
def do_left(self, rest):
t = self.clamp_time(rest)
self.issue_cmd((0.0, LR_SPEED, t))
self.has_cmd = True
do_lt = do_left
do_l = do_left
do_a = do_left
def do_right(self, rest):
t = self.clamp_time(rest)
self.issue_cmd((0.0, -LR_SPEED, t))
self.has_cmd = True
do_rt = do_right
do_r = do_right
do_d = do_right
def do_votes(self, rest):
try_message(CHANNEL, 'Votes: {}'.format(tbcmds.repr_votes()))
self.has_cmd = True
do_v = do_votes
tbcmds = TBotCmds()
class TIRCBot(pydle.Client):
def on_connect(self):
print('Joining...')
self.join(CHANNEL)
def on_message(self, target, source, message):
print('RECV:', target, ':', source, ':', message)
try:
tbcmds.context = source
parts = message.split(';')
for part in parts:
try:
tbcmds.onecmd(part)
except Exception:
traceback.print_exc()
if tbcmds.has_cmd:
tbcmds.has_cmd = False
except Exception:
traceback.print_exc()
if __name__ == '__main__':
tirc = TIRCBot('turtle_donatello')
print('Connecting...')
tirc.connect('irc.twitch.tv', 6667, password='nothing_to_see_here_move_on')
try:
tirc.handle_forever()
except KeyboardInterrupt:
tirc.message(CHANNEL, 'Software restarting, be back in a bit...')
time.sleep(2)
Loading…
Cancel
Save