Commit bf7c6418 authored by Nicholas Shindler's avatar Nicholas Shindler
Browse files

clean up errors

parent cde1b079
......@@ -10,3 +10,5 @@ from writer import *
from config import *
from utils import *
from listener import *
......@@ -2,13 +2,18 @@ import socket
import select
import struct
import code
import threading
import Queue
import threading
import time
import os
import sys
import subprocess
# local imports
from listener import *
from utils import *
from config import *
### --------- ###
#
### Harvest Vehicle Client Connection ###
......@@ -34,7 +39,9 @@ class Client:
self.probe_names = {}
self.probe_defs = {}
self.probe_list = []
self.capturing = False
self.listener_pipe = None
self.listener = None
self.connect(host)
......@@ -141,7 +148,7 @@ class Client:
try:
for id in self.probe_defs.keys():
(name, type, length) = self.probe_defs[id]
type_name = self.type_table[type][0]
type_name = type_table[type][0]
probe_code_list.append(name)
except Exception as e:
raise e
......@@ -181,25 +188,25 @@ class Client:
def encode_probe_value(self, probe_type, value):
# encode as tlv msg
if self.type_table[probe_type][0] == "tlv":
if type_table[probe_type][0] == "tlv":
return value # assume it's already encoded.
elif self.type_table[probe_type][0] == "string":
elif type_table[probe_type][0] == "string":
return value + "\0" # add a null terminator
else:
return struct.pack(self.type_table[probe_type][1], value)
return struct.pack(type_table[probe_type][1], value)
def decode_probe_data(self, probe_type, data):
# decode tlv msg
if self.type_table[probe_type][0] == "tlv":
if type_table[probe_type][0] == "tlv":
return tlv_to_text(data)
elif self.type_table[probe_type][0] == "string":
elif type_table[probe_type][0] == "string":
# return len(data)
return data[:-1] # lose the null terminator
else:
# try to unpack data
# NOTE: this was added to prevent total falure when packets type data failed (specifically the float type)
try:
data = struct.unpack(self.type_table[probe_type][1], data)[0]
data = struct.unpack(type_table[probe_type][1], data)[0]
return data
except:
raise RuntimeError("struct unpack failed")
......@@ -378,7 +385,7 @@ class Client:
self.listener_q = Queue.Queue(maxsize=n)
self.listener = self.listen(self.listener_q, self)
self.listener = listen(self.listener_q, self)
self.listener.start()
def stop_listener(self):
......@@ -388,6 +395,10 @@ class Client:
os.close(self.listener_pipe[1])
self.listener.join()
def addListenerCallback(self, funct):
# add a callback function to the listener
self.listener.addMsgCallback(funct)
## ----- Script Methods ----- ##
def send_script(self, scriptname):
......
from enum import Enum
###
# Type names for probe types (t in tlv)
###
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
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
}
# Declare function types
HV_Services = Enum(
"HV_Services",
"DRIVE_CMD GRIPPER_CMD ARM_CMD ODOM_CMD ODOM_LSTN IR_LSTN LIDAR_LSTN GRIPPER_LSTN NEAREST_OBS ODOM_GYRO_LSTN FOLLOW_SRV PICK_SRV AVOID_SRV",
)
# Function toggles
DRIVE_CMD = HV_Services.DRIVE_CMD
GRIPPER_CMD = HV_Services.GRIPPER_CMD
ARM_CMD = HV_Services.ARM_CMD
......@@ -20,16 +61,31 @@ PICK_SRV = HV_Services.PICK_SRV
AVOID_SRV = HV_Services.AVOID_SRV
# Probe names
START = "ui.fakeStartButtonNoSpacing" #1125
LIDAR_STANDBY = "vis.sick.useStandByMode" #2877
ARM_ENABLED = "manip.armEnableRequest" #1082
GRIPPER_ENABLED = "manip.gripperEnableRequest" #1083
DRIVE_ENABLED = "drive.enableRequest" #580
DRIVE_COMMAND = "mob.driveCommand.enable" #1220
ROB_SENSOR = "vis.robotSensor.enabled" #3100
LINE_SENSOR = "vis.lineSensor.enabled" #3171
ESTOP = "ui.fakeEstop"
FLAG_PULL = "estop.fakeFlagPulled"
BEEP = "ui.enableBeeping" #1120
HAS_POT = "manip.botGotPot"
IR_RIGHT_FRONT_A = "bound.rightFront.signalA"
IR_LEFT_FRONT_A = "bound.leftFront.signalA"
IR_RIGHT_BACK_A = "bound.rightBack.signalA"
IR_LEFT_BACK_A = "bound.leftBack.signalA"
IR_RIGHT_BACK_A = "bound.rightRear.signalA"
IR_LEFT_BACK_A = "bound.leftRear.signalA"
IR_RIGHT_FRONT_B = "bound.rightFront.signalB"
IR_LEFT_FRONT_B = "bound.leftFront.signalB"
IR_RIGHT_BACK_B = "bound.rightBack.signalB"
IR_LEFT_BACK_B = "bound.leftBack.signalB"
IR_RIGHT_BACK_B = "bound.rightRear.signalB"
IR_LEFT_BACK_B = "bound.leftRear.signalB"
LOCAL_SCAN = "vis.sick.localScanData"
OBSTACLE_X = "vis.forwardObstacle.x"
......
import socket
import select
import struct
import code
import threading
import Queue
import time
import os
import sys
import subprocess
import rospy
class listen(threading.Thread):
# listener class for getting data from robot
def __init__(self, q, client):
threading.Thread.__init__(self)
self.q = q
self.mp = mp
self.client = client
self.callback = None
self.setDaemon(True)
def addMsgCallback(callbackFcn):
def addMsgCallback(self, callbackFcn):
# adds a function that is called whenever a message is recived by the queue
self.callback = callbackFcn
......@@ -36,4 +48,5 @@ class listen(threading.Thread):
pass
# time.sleep(0.1)
except Exception as e:
rospy.logwarn(e)
raise e
......@@ -2,12 +2,13 @@ class Reader:
# reader class for getting data from the robot and passing to ROS
def __init__(self, p):
# constructor for settings class
self.pub = None
self.probes = p
self.queue = {}
for label in p:
self.queue[label] = 0
def pub(self, data):
def publish(self, data):
## Publish data
self.pub.publish(data)
......
import math
import struct
### various utility functions shared by multiple modules ###
......@@ -29,3 +30,49 @@ def twist_to_vel(twist):
def resource_name(name):
# formats a hostname to be legaly used in a ros pubisher/subscriber name
return ((name.replace("-", "")).replace(".", "_")).lower()
###
# functions that handle the formatting, reading and witing of tlv messages
# tlv is the structure of message that mp uses to communicate
# t = type
# l = length
# v = value
###
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=""):
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(message):
(t, l) = struct.unpack("<HH", message[0:4])
v = message[4 : 4 + l]
return (t, l, v, message[4 + l :])
def tlv_to_text(data):
(t, l) = struct.unpack("<HH", data[0:4])
v = data[4 : 4 + l]
# If we knew which t values were recursive encodings, we could
# be clever here.
string = "t=%d:l=%d:v=" % (t, l)
for i in range(len(v)):
string += "%02x" % ord(v[i])
if i < len(v):
string += ":"
return string
......@@ -16,8 +16,6 @@ class GripperListener(Reader):
# Gripper information class extending the getter, for handling the publishing of data
###
# HAS_POT = "manip.botGotPot"
def __init__(self, h):
self.hostname = h
Reader.__init__(self, [HAS_POT])
......@@ -27,9 +25,9 @@ class GripperListener(Reader):
queue_size=1,
)
def pub(self):
def publish(self):
## handles the publishing of data, should be run at an acceptable update interval
# get data from queue
data = Bool(self.q[HAS_POT])
data = Bool(self.queue[HAS_POT])
# publish data
Reader.pub(self, data)
Reader.publish(self, data)
......@@ -28,13 +28,13 @@ class IrListener(Reader):
queue_size=1,
)
def pub(self):
def publish(self):
## handles the publishing of data, should be run at an acceptable update interval
# get data from queue
data_list = [self.q[label] for label in self.probes]
data_list = [self.queue[label] for label in self.probes]
data = sum(data_list) / len(data_list)
# publish data
Reader.pub(self, data)
Reader.publish(self, data)
class IrListenerRightFront(IrListener):
......
......@@ -36,19 +36,19 @@ class LidarListener(Reader):
queue_size=1,
)
def pub(self):
def publish(self):
## handles the publishing of data, should be run at an acceptable update interval
# get data from queue
try:
data = self.q[LOCAL_SCAN]
data = self.queue[LOCAL_SCAN]
# print data
if type(data) != str:
raise Exception("Data not in correct format!")
raise TypeError("Data not in correct format!")
# get the data from the message
data = data.split("v=")[-1]
if data == "":
raise Exception("lidar data is empty")
raise TypeError("lidar data is empty")
# create x and y arrays from the base16 string of data
strarr = data.split(":")
......@@ -74,12 +74,13 @@ class LidarListener(Reader):
msg = pc2.create_cloud_xyz32(header, elem_arr)
# publish data
Reader.pub(self, msg)
Reader.publish(self, msg)
except TypeError:
pass
except Exception as e:
# well this is sad
err = traceback.format_exc()
print(err)
raise e
class LidarNObsListener(Reader):
......@@ -98,12 +99,12 @@ class LidarNObsListener(Reader):
queue_size=1,
)
def pub(self):
def publish(self):
## handles the publishing of data, should be run at an acceptable update interval
# get data from queue
try:
x = float(self.q[OBSTACLE_X])
y = float(self.q[OBSTACLE_Y])
x = float(self.queue[OBSTACLE_X])
y = float(self.queue[OBSTACLE_Y])
z = 0.0
msg = PointStamped()
......@@ -114,9 +115,8 @@ class LidarNObsListener(Reader):
msg.point.z = z
# publish data
Reader.pub(self, msg)
Reader.publish(self, msg)
except Exception as e:
# well this is sad
err = traceback.format_exc()
print(err)
raise e
......@@ -38,14 +38,14 @@ class Odometer(Reader):
queue_size=1,
)
def pub(self):
def publish(self):
## handles the publishing of data, should be run at an acceptable update interval
# get data from queue
odom = Odometry()
odom.header.stamp = rospy.get_rostime()
odom.header.frame_id = self.hostname
[X, Y, H, V, W] = list(map(self.q.get, Reader.get_probes(self)))
[X, Y, H, V, W] = list(map(self.queue.get, Reader.get_probes(self)))
heading = tf.transformations.quaternion_from_euler(0.0, 0.0, H)
odom.pose.pose = Pose(
......@@ -54,7 +54,7 @@ class Odometer(Reader):
)
odom.twist.twist = Twist(Vector3(V, 0, 0), Vector3(0, 0, W))
# publish data
Reader.pub(self, odom)
Reader.publish(self, odom)
class OdometryListener(Odometer):
......
......@@ -32,6 +32,7 @@ class HVClient(Client):
# constructor for the hv bridge instance of the client connection
def __init__(self, hostname, services):
self.hostname = hostname
name = resource_name(hostname)
if DRIVE_CMD in services:
# enable drive controller
......@@ -88,20 +89,20 @@ class HVClient(Client):
try:
# this will prevent the robot from emiting beeps
if silent:
self.write((BEEP, 0))
Client.write_probe(self, BEEP, 0)
# start the robot
self.write((FAKE_START, 1))
Client.write_probe(self, START, 1)
# this will start collection with the lidar sensor
if self.using_lidar:
self.write((LIDAR_STANDBY, 0))
Client.write_probe(self, LIDAR_STANDBY, 0)
self.enable_services()
self.listener.addMsgCallback(self.read)
Client.addListenerCallback(self, self.read)
self.start_probes(self)
Client.start_probes(self)
except Exception as e:
raise e
......@@ -111,21 +112,21 @@ class HVClient(Client):
# sets the start to FALSE. this 'stops' the robot.
# sets flag pulled to true
# TODO: switch codes for enums
self.write([(2426, 1), (FAKE_START, 0), (2419, 1)])
Client.write_probes(self,[(ESTOP, 1), (START, 0), (FLAG_PULL, 1)])
print "virtual e-stop triggered"
time.sleep(1)
self.write([(2426, 0), (2419, 0)])
Client.write_probes(self,[(ESTOP, 0), (FLAG_PULL, 0)])
print str(self.hostname) + " has stopped all running processes"
except Exception as e:
raise e
def enable_services():
def enable_services(self):
# take all probes that need to be listened for and enable them
try:
get_probe_arr = (
lambda a, b: get_probe_arr(a.union(b[0]), b[1:])
if len(b) > 1
else list(a.union(b[0]))
else tuple(a.union(b[0]))
)
probes = get_probe_arr(set([]), map((lambda a: set(a.probes)), self.lstnr))
......@@ -139,24 +140,24 @@ class HVClient(Client):
get_msg_arr = (
lambda a, b: get_msg_arr(a + b[0], b[1:]) if len(b) > 1 else a + b[0]
)
msgs = get_probe_arr([], map((lambda a: a.get_msgs()), self.cmdr))
msgs = get_msg_arr([], map((lambda a: a.get_msgs()), self.cmdr))
v = ""
if len(msgs) == 1:
msgs = ((msgs),)
for (probe, value) in msgs:
probe_id = self.lookup_probe_id(probe)
probe_id = Client.lookup_probe_id(self,probe)
probe_type = self.probe_defs[probe_id][1]
v += make_tlv(probe_id, self.encode_probe_value(probe_type, value))
self.send_message(make_tlv(MP_TLV_WRITE_PROBES, v))
v += make_tlv(probe_id, Client.encode_probe_value(self,probe_type, value))
Client.send_message(self,make_tlv(MP_TLV_WRITE_PROBES, v))
except Exception as e:
raise e
def read(self, t, v):
# pass data to the correct child class
try:
name = Client.probe_defs[t][0]
type = Client.probe_defs[t][1]
name = self.probe_defs[t][0]
type = self.probe_defs[t][1]
val = Client.decode_probe_data(self, type, v)
[serv.add(name, val) for serv in self.lstnr]
......@@ -167,7 +168,7 @@ class HVClient(Client):
def publish(self):
# publish the data to ROS
try:
for cmd in self.cmdr:
cmd.pub()
for lstn in self.lstnr:
lstn.publish()
except Exception as e:
raise e
......@@ -39,7 +39,6 @@ if __name__ == "__main__":
help="The name of the robot, generally of the form hai-####.local (default: hai-1095.local",
)
parser.add_argument(
"-a",
"--all",
action="store_true",
dest="all",
......@@ -84,6 +83,12 @@ if __name__ == "__main__":
default="all",
help="Set the services to make available",
)
parser.add_argument(
"sys",
type=str,
nargs="*",
help="System included variables from launch file (not set bu user)"
)
args = parser.parse_args()
......@@ -171,8 +176,8 @@ if __name__ == "__main__":
# set rate based on the mindprobe refresh rate.
# default to 100. (mp runs at 200)
hz = 100
if mp.hz:
hz = mp.hz
if hv_bot.hz:
hz = hv_bot.hz/2
# Start the ROS main loop
rate = rospy.Rate(hz)
......
<launch>
<node name="main_node.py" pkg="hv_bridge" type="bridge" args="--all" />
<node name="hv_bridge" pkg="hv_bridge" type="main_node.py" args=" --all hai-1095.local" />
</launch>
<?xml version="1.0"?>
<package format="2">
<name>hai_launch</name>
<name>hv_launch</name>
<version>0.0.0</version>
<description>The hai_launch package</description>
<description>The hv_launch package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
......
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