Votes Internally!
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

194 lines
4.4 KiB

#!/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)