Commit 0b057173 authored by Nicholas Shindler's avatar Nicholas Shindler
Browse files

reformat files

parent a4067e6c
......@@ -42,7 +42,7 @@ class Client:
self.capturing = False
self.listener_pipe = None
self.listener = None
self.found_probes=False
self.found_probes = False
self.listener_closed = True
self.connect(host)
......@@ -87,7 +87,7 @@ class Client:
self.discover_probes()
time.sleep(0.1)
# toggle state to record that the probes have been looked up and stored
self.found_probes=True
self.found_probes = True
print "Protocol version = 0x%04x, sample rate is %d Hz, %d probes" % (
self.version,
......@@ -102,19 +102,19 @@ class Client:
try:
# end the connection
self.stop_listener()
while(not self.listener_closed):
while not self.listener_closed:
time.sleep(0.5)
self.s.close()
print "Client has disconnected"
except Exception as e:
print "Client Disconnet Failed"
raise e
def reconnect(self, host, port=4400):
# attempt to reconnect the client to the server
try:
# clear up previous listener/socket connection
#self.listener.join()
# self.listener.join()
self.s.close()
self.s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
......@@ -234,12 +234,12 @@ class Client:
raise RuntimeWarning("Probe list has not been discovered yet")
if type(t) != int:
raise ValueError("probe id must be an int")
#print(t in self.probe_defs.keys())
if (t not in self.probe_defs.keys()):
# print(t in self.probe_defs.keys())
if t not in self.probe_defs.keys():
raise LookupError("Probe %i not found in probe defs" % t)
return (self.probe_defs[t][0], self.probe_defs[t][1])
except Exception:
raise
raise
def enable_probes(self, probes):
# enables one or more probes.
......@@ -344,7 +344,7 @@ class Client:
message_text = v[0 : len(v) - 1]
if message_text[-1] == "\n":
message_text = message_text[0 : len(message_text) - 1]
return ("Debug message (t = %d): %s" % (tick, message_text))
return "Debug message (t = %d): %s" % (tick, message_text)
def do_async_message(self, t, l, v):
# async msg handler
......@@ -476,7 +476,7 @@ class Client:
# close listener connection
# TODO: why does this line fail?
try:
os.write(self.listener_pipe[1], 'bye')
os.write(self.listener_pipe[1], "bye")
os.close(self.listener_pipe[1])
except Exception as e:
print "listener pipe not closing"
......
......@@ -13,7 +13,6 @@ import traceback
from socket import error as SocketError
class listen(threading.Thread):
# listener class for getting data from robot
def __init__(self, q, client):
......@@ -53,4 +52,4 @@ class listen(threading.Thread):
err = traceback.format_exc()
rospy.logerr(e)
rospy.loginfo(err)
#raise
# raise
......@@ -107,11 +107,11 @@ class HVClient(Client):
time.sleep(1)
# if data needs to be collected from the robot, enable the probes
if(len(self.lstnr) != 0):
if len(self.lstnr) != 0:
self.enable_services()
time.sleep(1)
#Client.start_probes(self)
# Client.start_probes(self)
except Exception as e:
err = traceback.format_exc()
rospy.logerr(err)
......@@ -176,21 +176,21 @@ class HVClient(Client):
lambda a, b: get_msg_arr(a + b[0], b[1:]) if len(b) > 1 else a + b[0]
)
# collect all subscription messages to write to robot
if(len(self.cmdr) > 0):
if len(self.cmdr) > 0:
msgs = get_msg_arr([], map((lambda a: a.get_msgs()), self.cmdr))
# collect all services messages to write to robot
if(len(self.srvs) > 0):
if len(self.srvs) > 0:
msgs += get_msg_arr([], map((lambda a: a.get_msgs()), self.srvs))
v = ""
if len(msgs) == 0:
return
for (probe, value) in msgs:
value_type = type(value)
if(value_type == tuple or value_type == list):
#raise TypeError("message value incorrectly formatted")
if value_type == tuple or value_type == list:
# raise TypeError("message value incorrectly formatted")
rospy.logerr("Probe [%s] has an incorrectly formatted value", probe)
continue
if(value_type != str and value_type != int and value_type != float):
if value_type != str and value_type != int and value_type != float:
rospy.logerr("Probe [%s] has an incorrectly type value", probe)
continue
probe_id = Client.lookup_probe_id(self, probe)
......@@ -205,7 +205,7 @@ class HVClient(Client):
if e.errno == errno.ECONNRESET:
rospy.logerr("CONNECTION RESET BY PEER WHILE WRITING TO SERVER")
rospy.loginfo("Reconnecting to Server")
#self.restart()
# self.restart()
Client.reconnect(self, self.hostname)
else:
raise e
......@@ -236,25 +236,29 @@ class HVClient(Client):
(t, l, v, data) = next_tlv(data)
(probe_name, probe_type) = Client.lookup_probe(self, t)
if any([lst.needs(probe_name) for lst in self.lstnr]):
try:
try:
# attempt to decode the probe data
val = Client.decode_probe_data(self, probe_type, v)
#print ("New Message[name: " + str(probe_name) + " val: " + str(val) + "]")
# print ("New Message[name: " + str(probe_name) + " val: " + str(val) + "]")
# if the data is valid assign it to relavent listeners
[serv.add(probe_name, val) for serv in self.lstnr]
except RuntimeError as e:
# Catch the runtime error emitted when the probe data decoding fails
rospy.logwarn("Error processing probe data of type %s", type_table[probe_type][0])
rospy.logwarn(
"Error processing probe data of type %s",
type_table[probe_type][0],
)
err = traceback.format_exc()
rospy.logwarn(err)
continue
else:
# msg can be used for debugging. otherwise this block is probably not needed
# rospy.loginfo("Probe info not needed: %s" % probe_name)
rospy.logdebug("Probe info not needed: %s" % probe_name)
pass
except RuntimeWarning as e:
#rospy.logwarn(e)
# msg can be used for debugging. otherwise this block is probably not needed
rospy.logdebug(e)
pass
except Exception as e:
err = traceback.format_exc()
......
......@@ -202,7 +202,7 @@ if __name__ == "__main__":
rospy.loginfo(err)
if e.errno == errno.ECONNRESET:
rospy.logerr("CONNECTION RESET BY PEER")
if(hv_bot != None and not hv_bot.stopped):
if hv_bot != None and not hv_bot.stopped:
rospy.loginfo("Reconnecting to Server")
hv_bot.reconnect(host_name)
except SocketError as e:
......@@ -234,7 +234,7 @@ if __name__ == "__main__":
pass
finally:
# last code to run before everything closes
if(hv_bot != None and not hv_bot.stopped):
if hv_bot != None and not hv_bot.stopped:
rospy.loginfo("Stopping Client")
hv_bot.stop()
rospy.loginfo("GOODBYE")
......@@ -8,24 +8,27 @@ from geometry_msgs.msg import Twist
from std_msgs.msg import Float32, Float64
import sys, os, select, termios, tty, math
import argparse,time
import argparse, time
def resource_name(name):
# formats a hostname to be legaly used in a ros pubisher/subscriber name
return ((name.replace("-", "")).replace(".", "_")).lower()
def progressbar(it, prefix="", size=60, file=sys.stdout):
count=len(it)
count = len(it)
def show(j):
x=int(size*j/count)
p=int(100*j/count)
file.write("%s[%s%s] %i%s\r" % (prefix, "#"*x, "."*(size-x),p,"%"))
x = int(size * j / count)
p = int(100 * j / count)
file.write("%s[%s%s] %i%s\r" % (prefix, "#" * x, "." * (size - x), p, "%"))
file.flush()
show(0)
for i, item in enumerate(it):
yield item
show(i+1)
show(i + 1)
file.write("\n")
file.flush()
......@@ -187,27 +190,23 @@ if __name__ == "__main__":
# set the hostname based on the argument inputed by the user (default: hai-1095.local)
host_name = args.hostname
if(len(args.sys) > 0):
if len(args.sys) > 0:
# started from launch file - need to wait for bridge to connect
for i in progressbar(range(100), "STARTING: ", 50):
time.sleep(0.1)
print("Controling %s robot" % host_name)
settings = termios.tcgetattr(sys.stdin)
name = resource_name(host_name)
# create the 3 publishers
pub_drive = rospy.Publisher(
"/" + name + "/cmd_vel", Twist, queue_size=1
)
pub_drive = rospy.Publisher("/" + name + "/cmd_vel", Twist, queue_size=1)
pub_gripper = rospy.Publisher(
"/" + name + "/set_gripper_pos", Float32, queue_size=1
)
pub_arm = rospy.Publisher(
"/" + name + "/set_arm_pos", Float32, queue_size=1
)
pub_arm = rospy.Publisher("/" + name + "/set_arm_pos", Float32, queue_size=1)
# init ros
rospy.init_node("hv_teleop_node", anonymous=True)
......
# mp-test.py -- tests for the mindprobe running under the BTB
from mpclient import *
......@@ -6,27 +5,25 @@ import sys
import unittest
class TestParameters(unittest.TestCase):
def setUp(self):
btb.clear_world()
btb.set_default_params()
def testSimpleParams(self):
"""Test that simple parameters can be set to non-default values."""
simple_params = ("robot_crash_radius",
"robot_wheelbase",
"robot_wheel_diameter",
"robot_wheel_clicks",
"robot_max_acceleration",
"robot_motor_command_max",
"max_run_time",
"ticks_per_sec",
"field_radius",
"pot_radius")
simple_params = (
"robot_crash_radius",
"robot_wheelbase",
"robot_wheel_diameter",
"robot_wheel_clicks",
"robot_max_acceleration",
"robot_motor_command_max",
"max_run_time",
"ticks_per_sec",
"field_radius",
"pot_radius",
)
for param in simple_params:
exec "x0 = get_%s()" % param
......@@ -35,7 +32,6 @@ class TestParameters(unittest.TestCase):
exec "x1 = get_%s()" % param
self.assertEqual(x1, x0 + 1)
def testBoundaryParams(self):
"""Test that the boundary can be set."""
(x0, y0, x1, y1) = get_boundary_location()
......@@ -47,8 +43,8 @@ class TestParameters(unittest.TestCase):
self.assertEqual(x1b, x1 + 3)
self.assertEqual(y1b, y1 - 4)
class TestRobotMotion(unittest.TestCase):
def setUp(self):
btb.clear_world()
btb.set_default_params()
......@@ -56,22 +52,22 @@ class TestRobotMotion(unittest.TestCase):
self.x0 = 1
self.y0 = 1
self.h0 = deg2rad(45)
self.r = create_robot(x = self.x0, y = self.y0, heading = self.h0)
self.r = create_robot(x=self.x0, y=self.y0, heading=self.h0)
def testRobotStopped(self):
"""Test that a robot stopped does not move."""
send_magic_message(self.r, "stop")
run_world(seconds = 1)
run_world(seconds=1)
(x, y, heading) = get_robot_position(self.r)
self.assertEqual(x, self.x0)
self.assertEqual(y, self.y0)
self.assertEqual(heading, deg2rad(45))
def testRobotForward(self):
"""Test that a robot moves forward"""
send_magic_message(self.r, "forward")
send_magic_message(self.r, "slow")
run_world(seconds = 1)
run_world(seconds=1)
(x, y, heading) = get_robot_position(self.r)
self.assertEqual(x, y)
self.assert_(x > self.x0)
......@@ -81,7 +77,7 @@ class TestRobotMotion(unittest.TestCase):
"""Test that a robot turns left"""
send_magic_message(self.r, "left")
send_magic_message(self.r, "slow")
run_world(seconds = 1)
run_world(seconds=1)
(x, y, heading) = get_robot_position(self.r)
self.assert_(heading > self.h0)
self.assert_(x > self.x0)
......@@ -91,7 +87,7 @@ class TestRobotMotion(unittest.TestCase):
"""Test that a robot turns left"""
send_magic_message(self.r, "right")
send_magic_message(self.r, "slow")
run_world(seconds = 1)
run_world(seconds=1)
(x, y, heading) = get_robot_position(self.r)
self.assert_(heading < self.h0)
self.assert_(x > self.x0)
......@@ -99,7 +95,6 @@ class TestRobotMotion(unittest.TestCase):
class TestRobotErrors(unittest.TestCase):
def setUp(self):
btb.clear_world()
btb.set_default_params()
......@@ -107,8 +102,8 @@ class TestRobotErrors(unittest.TestCase):
def testRobotRobotCrash(self):
"""Test that robots crashing generates an exception"""
r0 = create_robot(x = 0, y = 0, heading = 0)
r1 = create_robot(x = 2, y = 0, heading = 0)
r0 = create_robot(x=0, y=0, heading=0)
r1 = create_robot(x=2, y=0, heading=0)
send_magic_message(r0, "forward")
send_magic_message(r0, "slow")
......@@ -135,7 +130,7 @@ class TestRobotErrors(unittest.TestCase):
field_radius = 3
r0 = create_robot(x = 0, y = 0, heading = 0)
r0 = create_robot(x=0, y=0, heading=0)
set_field_radius(field_radius)
......@@ -154,5 +149,5 @@ class TestRobotErrors(unittest.TestCase):
self.assert_(distance(0, 0, x0, y0) >= field_radius)
if __name__ == '__main__':
if __name__ == "__main__":
unittest.main()
#
# mp-client.py -- dummy mindprobe client for testing
#
......@@ -14,45 +13,46 @@ import os
import sys
import subprocess
MP_TLV_NULL = 0
MP_TLV_CONNECTED = 1
MP_TLV_DISCONNECTED = 2
MP_TLV_PROTOCOL_VERSION = 3
MP_TLV_DISCOVER_PROBES = 4
MP_TLV_HZ = 5
MP_TLV_PROBE_DEF = 6
MP_TLV_ENABLE_PROBES = 7
MP_TLV_START_PROBES = 8
MP_TLV_STOP_PROBES = 9
MP_TLV_PROBE_DATA = 10
MP_TLV_CURRENT_TICK = 11
MP_TLV_MISSED_DATA = 12
MP_TLV_DEBUG_MESSAGE = 13
MP_TLV_MESSAGE_TEXT = 14
MP_TLV_NULL = 0
MP_TLV_CONNECTED = 1
MP_TLV_DISCONNECTED = 2
MP_TLV_PROTOCOL_VERSION = 3
MP_TLV_DISCOVER_PROBES = 4
MP_TLV_HZ = 5
MP_TLV_PROBE_DEF = 6
MP_TLV_ENABLE_PROBES = 7
MP_TLV_START_PROBES = 8
MP_TLV_STOP_PROBES = 9
MP_TLV_PROBE_DATA = 10
MP_TLV_CURRENT_TICK = 11
MP_TLV_MISSED_DATA = 12
MP_TLV_DEBUG_MESSAGE = 13
MP_TLV_MESSAGE_TEXT = 14
MP_TLV_MISSED_DEBUG_MESSAGES = 15
MP_TLV_WRITE_PROBES = 16
MP_TLV_WRITE_PROBES = 16
# Type names for probe type enumerations, and pack/unpack directives:
type_table = {
1: ('uint8', '<B'), # 1-byte unsigned integer
2: ('int8', '<b'), # 1-byte signed integer
3: ('uint16', '<H'), # 2-byte unsigned integer
4: ('int16', '<h'), # 2-byte signed integer
5: ('uint32', '<I'), # 4-byte unsigned integer
6: ('int32', '<i'), # 4-byte signed integer
7: ('uint64', '<Q'), # 8-byte unsigned integer
8: ('int64', '<q'), # 8-byte signed integer
9: ('float', '<f'), # 4-byte float (IEEE 754)
10: ('double', '<d'), # 8-byte float (IEEE 754)
11: ('tlv', ''), # Variable-length TLV data
12: ('bool', '<B'), # 1-byte boolean integer (1 or 0)
13: ('string', ''), # variable-size null-terminated char string
1: ("uint8", "<B"), # 1-byte unsigned integer
2: ("int8", "<b"), # 1-byte signed integer
3: ("uint16", "<H"), # 2-byte unsigned integer
4: ("int16", "<h"), # 2-byte signed integer
5: ("uint32", "<I"), # 4-byte unsigned integer
6: ("int32", "<i"), # 4-byte signed integer
7: ("uint64", "<Q"), # 8-byte unsigned integer
8: ("int64", "<q"), # 8-byte signed integer
9: ("float", "<f"), # 4-byte float (IEEE 754)
10: ("double", "<d"), # 8-byte float (IEEE 754)
11: ("tlv", ""), # Variable-length TLV data
12: ("bool", "<B"), # 1-byte boolean integer (1 or 0)
13: ("string", ""), # variable-size null-terminated char string
}
capture_buffer = ()
def connect(host, port = 4400):
def connect(host, port=4400):
global s, recvbuf, probe_defs, hz, version
global capture_buffer_lock
......@@ -60,7 +60,7 @@ def connect(host, port = 4400):
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.connect((host, port))
recvbuf = ''
recvbuf = ""
print "Connected"
start_listener()
......@@ -68,63 +68,68 @@ def connect(host, port = 4400):
# Expect the server to send us the protocol version, unsolicited:
(t, l, v) = get_next_message()
if (t != MP_TLV_PROTOCOL_VERSION) or (l != 2):
raise RuntimeError('unexpected message t=%d l=%d' % (t, l))
raise RuntimeError("unexpected message t=%d l=%d" % (t, l))
version = struct.unpack("<H", v)[0]
# Query the tick rate:
send_message(make_tlv(MP_TLV_HZ))
(t, l, v) = get_next_message()
if (t != MP_TLV_HZ) or (l != 2):
raise RuntimeError('unexpected message t=%d l=%d' % (t, l))
raise RuntimeError("unexpected message t=%d l=%d" % (t, l))
hz = struct.unpack("<H", v)[0]
# Discover the probes:
discover_probes()
print "Protocol version = 0x%04x, sample rate is %d Hz, %d probes" % \
(version, hz, len(probe_defs.keys()))
print "Protocol version = 0x%04x, sample rate is %d Hz, %d probes" % (
version,
hz,
len(probe_defs.keys()),
)
def disconnect():
global s
stop_listener()
s.close()
def discover_probes():
global probe_defs, probe_names, version
new_probe_defs = {} # dictionary of id: (name, type)
new_probe_names = {} # dictionary of name: id
new_probe_names = {} # dictionary of name: id
send_message(make_tlv(MP_TLV_DISCOVER_PROBES))
while True:
(t, l, def_items) = get_next_message()
if (t != MP_TLV_DISCOVER_PROBES):
raise RuntimeError('unexpected message t=%d l=%d' % (t, l))
if t != MP_TLV_DISCOVER_PROBES:
raise RuntimeError("unexpected message t=%d l=%d" % (t, l))
while len(def_items):
(t, l, v, def_items) = next_tlv(def_items)
if t != MP_TLV_PROBE_DEF:
raise RuntimeError('unexpected message t=%d l=%d' % (t, l))
raise RuntimeError("unexpected message t=%d l=%d" % (t, l))
(probe_id, probe_type, probe_length) = struct.unpack("<HHH", v[0:6])
# Name has null terminator, which Python doesn't need.
probe_name = v[6:len(v)-1]
probe_name = v[6 : len(v) - 1]
new_probe_names[probe_name] = probe_id
new_probe_defs[probe_id] = (probe_name, probe_type, probe_length)
pass
if (len(def_items) == 0 or
version < 0x0104):
if len(def_items) == 0 or version < 0x0104:
probe_defs = new_probe_defs
probe_names = new_probe_names
break
pass
return
def show_probes():
global probe_defs
......@@ -133,26 +138,33 @@ def show_probes():
type_name = type_table[type][0]
print "probe id %d: %s type %s length %d" % (id, name, type_name, length)
def make_tlv_uint8(t, v):
return struct.pack("<HHH", t, 1, v)
def make_tlv_uint16(t, v):
return struct.pack("<HHH", t, 2, v)
def make_tlv_uint32(t, v):
return struct.pack("<HHI", t, 4, v)
def make_tlv(t, v = ''):
def make_tlv(t, v=""):
return struct.pack("<HH", t, len(v)) + v
def tlv_len(message):
(t, l) = struct.unpack("<HH", message[0:4])
return l
def next_tlv(