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

clean up errors

parent cde1b079
...@@ -10,3 +10,5 @@ from writer import * ...@@ -10,3 +10,5 @@ from writer import *
from config import * from config import *
from utils import * from utils import *
from listener import *
...@@ -2,13 +2,18 @@ import socket ...@@ -2,13 +2,18 @@ import socket
import select import select
import struct import struct
import code import code
import threading
import Queue import Queue
import threading
import time import time
import os import os
import sys import sys
import subprocess import subprocess
# local imports
from listener import *
from utils import *
from config import *
### --------- ### ### --------- ###
# #
### Harvest Vehicle Client Connection ### ### Harvest Vehicle Client Connection ###
...@@ -34,7 +39,9 @@ class Client: ...@@ -34,7 +39,9 @@ class Client:
self.probe_names = {} self.probe_names = {}
self.probe_defs = {} self.probe_defs = {}
self.probe_list = [] self.probe_list = []
self.capturing = False
self.listener_pipe = None self.listener_pipe = None
self.listener = None
self.connect(host) self.connect(host)
...@@ -141,7 +148,7 @@ class Client: ...@@ -141,7 +148,7 @@ class Client:
try: try:
for id in self.probe_defs.keys(): for id in self.probe_defs.keys():
(name, type, length) = self.probe_defs[id] (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) probe_code_list.append(name)
except Exception as e: except Exception as e:
raise e raise e
...@@ -181,25 +188,25 @@ class Client: ...@@ -181,25 +188,25 @@ class Client:
def encode_probe_value(self, probe_type, value): def encode_probe_value(self, probe_type, value):
# encode as tlv msg # 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. 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 return value + "\0" # add a null terminator
else: 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): def decode_probe_data(self, probe_type, data):
# decode tlv msg # decode tlv msg
if self.type_table[probe_type][0] == "tlv": if type_table[probe_type][0] == "tlv":
return tlv_to_text(data) return tlv_to_text(data)
elif self.type_table[probe_type][0] == "string": elif type_table[probe_type][0] == "string":
# return len(data) # return len(data)
return data[:-1] # lose the null terminator return data[:-1] # lose the null terminator
else: else:
# try to unpack data # try to unpack data
# NOTE: this was added to prevent total falure when packets type data failed (specifically the float type) # NOTE: this was added to prevent total falure when packets type data failed (specifically the float type)
try: try:
data = struct.unpack(self.type_table[probe_type][1], data)[0] data = struct.unpack(type_table[probe_type][1], data)[0]
return data return data
except: except:
raise RuntimeError("struct unpack failed") raise RuntimeError("struct unpack failed")
...@@ -378,7 +385,7 @@ class Client: ...@@ -378,7 +385,7 @@ class Client:
self.listener_q = Queue.Queue(maxsize=n) 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() self.listener.start()
def stop_listener(self): def stop_listener(self):
...@@ -388,6 +395,10 @@ class Client: ...@@ -388,6 +395,10 @@ class Client:
os.close(self.listener_pipe[1]) os.close(self.listener_pipe[1])
self.listener.join() self.listener.join()
def addListenerCallback(self, funct):
# add a callback function to the listener
self.listener.addMsgCallback(funct)
## ----- Script Methods ----- ## ## ----- Script Methods ----- ##
def send_script(self, scriptname): def send_script(self, scriptname):
......
from enum import Enum 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 = Enum(
"HV_Services", "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", "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 DRIVE_CMD = HV_Services.DRIVE_CMD
GRIPPER_CMD = HV_Services.GRIPPER_CMD GRIPPER_CMD = HV_Services.GRIPPER_CMD
ARM_CMD = HV_Services.ARM_CMD ARM_CMD = HV_Services.ARM_CMD
...@@ -20,16 +61,31 @@ PICK_SRV = HV_Services.PICK_SRV ...@@ -20,16 +61,31 @@ PICK_SRV = HV_Services.PICK_SRV
AVOID_SRV = HV_Services.AVOID_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" HAS_POT = "manip.botGotPot"
IR_RIGHT_FRONT_A = "bound.rightFront.signalA" IR_RIGHT_FRONT_A = "bound.rightFront.signalA"
IR_LEFT_FRONT_A = "bound.leftFront.signalA" IR_LEFT_FRONT_A = "bound.leftFront.signalA"
IR_RIGHT_BACK_A = "bound.rightBack.signalA" IR_RIGHT_BACK_A = "bound.rightRear.signalA"
IR_LEFT_BACK_A = "bound.leftBack.signalA" IR_LEFT_BACK_A = "bound.leftRear.signalA"
IR_RIGHT_FRONT_B = "bound.rightFront.signalB" IR_RIGHT_FRONT_B = "bound.rightFront.signalB"
IR_LEFT_FRONT_B = "bound.leftFront.signalB" IR_LEFT_FRONT_B = "bound.leftFront.signalB"
IR_RIGHT_BACK_B = "bound.rightBack.signalB" IR_RIGHT_BACK_B = "bound.rightRear.signalB"
IR_LEFT_BACK_B = "bound.leftBack.signalB" IR_LEFT_BACK_B = "bound.leftRear.signalB"
LOCAL_SCAN = "vis.sick.localScanData" LOCAL_SCAN = "vis.sick.localScanData"
OBSTACLE_X = "vis.forwardObstacle.x" 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): class listen(threading.Thread):
# listener class for getting data from robot # listener class for getting data from robot
def __init__(self, q, client): def __init__(self, q, client):
threading.Thread.__init__(self) threading.Thread.__init__(self)
self.q = q self.q = q
self.mp = mp self.client = client
self.callback = None self.callback = None
self.setDaemon(True) self.setDaemon(True)
def addMsgCallback(callbackFcn): def addMsgCallback(self, callbackFcn):
# adds a function that is called whenever a message is recived by the queue # adds a function that is called whenever a message is recived by the queue
self.callback = callbackFcn self.callback = callbackFcn
...@@ -36,4 +48,5 @@ class listen(threading.Thread): ...@@ -36,4 +48,5 @@ class listen(threading.Thread):
pass pass
# time.sleep(0.1) # time.sleep(0.1)
except Exception as e: except Exception as e:
rospy.logwarn(e)
raise e raise e
...@@ -2,12 +2,13 @@ class Reader: ...@@ -2,12 +2,13 @@ class Reader:
# reader class for getting data from the robot and passing to ROS # reader class for getting data from the robot and passing to ROS
def __init__(self, p): def __init__(self, p):
# constructor for settings class # constructor for settings class
self.pub = None
self.probes = p self.probes = p
self.queue = {} self.queue = {}
for label in p: for label in p:
self.queue[label] = 0 self.queue[label] = 0
def pub(self, data): def publish(self, data):
## Publish data ## Publish data
self.pub.publish(data) self.pub.publish(data)
......
import math import math
import struct
### various utility functions shared by multiple modules ### ### various utility functions shared by multiple modules ###
...@@ -29,3 +30,49 @@ def twist_to_vel(twist): ...@@ -29,3 +30,49 @@ def twist_to_vel(twist):
def resource_name(name): def resource_name(name):
# formats a hostname to be legaly used in a ros pubisher/subscriber name # formats a hostname to be legaly used in a ros pubisher/subscriber name
return ((name.replace("-", "")).replace(".", "_")).lower() 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): ...@@ -16,8 +16,6 @@ class GripperListener(Reader):
# Gripper information class extending the getter, for handling the publishing of data # Gripper information class extending the getter, for handling the publishing of data
### ###
# HAS_POT = "manip.botGotPot"
def __init__(self, h): def __init__(self, h):
self.hostname = h self.hostname = h
Reader.__init__(self, [HAS_POT]) Reader.__init__(self, [HAS_POT])
...@@ -27,9 +25,9 @@ class GripperListener(Reader): ...@@ -27,9 +25,9 @@ class GripperListener(Reader):
queue_size=1, queue_size=1,
) )
def pub(self): def publish(self):
## handles the publishing of data, should be run at an acceptable update interval ## handles the publishing of data, should be run at an acceptable update interval
# get data from queue # get data from queue
data = Bool(self.q[HAS_POT]) data = Bool(self.queue[HAS_POT])
# publish data # publish data
Reader.pub(self, data) Reader.publish(self, data)
...@@ -28,13 +28,13 @@ class IrListener(Reader): ...@@ -28,13 +28,13 @@ class IrListener(Reader):
queue_size=1, queue_size=1,
) )
def pub(self): def publish(self):
## handles the publishing of data, should be run at an acceptable update interval ## handles the publishing of data, should be run at an acceptable update interval
# get data from queue # 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) data = sum(data_list) / len(data_list)
# publish data # publish data
Reader.pub(self, data) Reader.publish(self, data)
class IrListenerRightFront(IrListener): class IrListenerRightFront(IrListener):
......
...@@ -36,19 +36,19 @@ class LidarListener(Reader): ...@@ -36,19 +36,19 @@ class LidarListener(Reader):
queue_size=1, queue_size=1,
) )
def pub(self): def publish(self):
## handles the publishing of data, should be run at an acceptable update interval ## handles the publishing of data, should be run at an acceptable update interval
# get data from queue # get data from queue
try: try:
data = self.q[LOCAL_SCAN] data = self.queue[LOCAL_SCAN]
# print data # print data
if type(data) != str: if type(data) != str:
raise Exception("Data not in correct format!") raise TypeError("Data not in correct format!")
# get the data from the message # get the data from the message
data = data.split("v=")[-1] data = data.split("v=")[-1]
if data == "": 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 # create x and y arrays from the base16 string of data
strarr = data.split(":") strarr = data.split(":")
...@@ -74,12 +74,13 @@ class LidarListener(Reader): ...@@ -74,12 +74,13 @@ class LidarListener(Reader):
msg = pc2.create_cloud_xyz32(header, elem_arr) msg = pc2.create_cloud_xyz32(header, elem_arr)
# publish data # publish data
Reader.pub(self, msg) Reader.publish(self, msg)
except TypeError:
pass
except Exception as e: except Exception as e:
# well this is sad # well this is sad
err = traceback.format_exc() raise e
print(err)
class LidarNObsListener(Reader): class LidarNObsListener(Reader):
...@@ -98,12 +99,12 @@ class LidarNObsListener(Reader): ...@@ -98,12 +99,12 @@ class LidarNObsListener(Reader):
queue_size=1, queue_size=1,
) )
def pub(self): def publish(self):
## handles the publishing of data, should be run at an acceptable update interval ## handles the publishing of data, should be run at an acceptable update interval
# get data from queue # get data from queue
try: try:
x = float(self.q[OBSTACLE_X]) x = float(self.queue[OBSTACLE_X])
y = float(self.q[OBSTACLE_Y]) y = float(self.queue[OBSTACLE_Y])
z = 0.0 z = 0.0
msg = PointStamped() msg = PointStamped()
...@@ -114,9 +115,8 @@ class LidarNObsListener(Reader): ...@@ -114,9 +115,8 @@ class LidarNObsListener(Reader):
msg.point.z = z msg.point.z = z
# publish data # publish data
Reader.pub(self, msg) Reader.publish(self, msg)
except Exception as e: except Exception as e:
# well this is sad # well this is sad
err = traceback.format_exc() raise e
print(err)
...@@ -38,14 +38,14 @@ class Odometer(Reader): ...@@ -38,14 +38,14 @@ class Odometer(Reader):
queue_size=1, queue_size=1,
) )
def pub(self): def publish(self):
## handles the publishing of data, should be run at an acceptable update interval ## handles the publishing of data, should be run at an acceptable update interval
# get data from queue # get data from queue
odom = Odometry() odom = Odometry()
odom.header.stamp = rospy.get_rostime() odom.header.stamp = rospy.get_rostime()
odom.header.frame_id = self.hostname 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) heading = tf.transformations.quaternion_from_euler(0.0, 0.0, H)
odom.pose.pose = Pose( odom.pose.pose = Pose(
...@@ -54,7 +54,7 @@ class Odometer(Reader): ...@@ -54,7 +54,7 @@ class Odometer(Reader):
) )
odom.twist.twist = Twist(Vector3(V, 0, 0), Vector3(0, 0, W)) odom.twist.twist = Twist(Vector3(V, 0, 0), Vector3(0, 0, W))
# publish data # publish data
Reader.pub(self, odom) Reader.publish(self, odom)
class OdometryListener(Odometer): class OdometryListener(Odometer):
......
...@@ -32,6 +32,7 @@ class HVClient(Client): ...@@ -32,6 +32,7 @@ class HVClient(Client):
# constructor for the hv bridge instance of the client connection # constructor for the hv bridge instance of the client connection
def __init__(self, hostname, services): def __init__(self, hostname, services):
self.hostname = hostname
name = resource_name(hostname) name = resource_name(hostname)
if DRIVE_CMD in services: if DRIVE_CMD in services:
# enable drive controller # enable drive controller
...@@ -88,20 +89,20 @@ class HVClient(Client): ...@@ -88,20 +89,20 @@ class HVClient(Client):
try: try:
# this will prevent the robot from emiting beeps # this will prevent the robot from emiting beeps
if silent: if silent:
self.write((BEEP, 0)) Client.write_probe(self, BEEP, 0)
# start the robot # start the robot
self.write((FAKE_START, 1)) Client.write_probe(self, START, 1)
# this will start collection with the lidar sensor # this will start collection with the lidar sensor
if self.using_lidar: if self.using_lidar:
self.write((LIDAR_STANDBY, 0)) Client.write_probe(self, LIDAR_STANDBY, 0)
self.enable_services() self.enable_services()