Commit 2343dc1f authored by Nicholas Shindler's avatar Nicholas Shindler
Browse files

lidar working, client class refactored

parent a066f35e
......@@ -2,9 +2,10 @@
from std_msgs.msg import Float32
import sys, os
SCRIPTS_PATH = os.path.abspath(os.path.join(sys.path[0], "../../../resources"))
SCRIPTS_PATH = os.path.abspath(os.path.join(sys.path[0], "../../"))
sys.path.append(SCRIPTS_PATH)
from setter import Setter
from libs.Resources import Setter
class ArmController(Setter):
......
......@@ -4,10 +4,11 @@ from std_msgs.msg import Float64
import sys, os
SCRIPTS_PATH = os.path.abspath(os.path.join(sys.path[0], "../../../resources"))
SCRIPTS_PATH = os.path.abspath(os.path.join(sys.path[0], "../../"))
sys.path.append(SCRIPTS_PATH)
from setter import Setter
from utils import *
from libs.Resources import Setter
from libs.Resources.utils import *
class DriveController(Setter):
......
......@@ -2,9 +2,10 @@
from std_msgs.msg import Float32
import sys, os
SCRIPTS_PATH = os.path.abspath(os.path.join(sys.path[0], "../../../resources"))
SCRIPTS_PATH = os.path.abspath(os.path.join(sys.path[0], "../../"))
sys.path.append(SCRIPTS_PATH)
from setter import Setter
from libs.Resources import Setter
class GripperController(Setter):
......
......@@ -5,3 +5,6 @@
###
from utils import *
from getter import *
from setter import *
from mp import *
......@@ -29,45 +29,8 @@ import subprocess
class Client:
def __init__(self, services):
def __init__(self):
try:
# enable services, selection default is all (0)
self.cmdr = []
self.lstnr = []
self.using_lidar = False
if DRIVE_CMD in services:
# enable drive controller
self.cmdr.append(DriveController())
if GRIPPER_CMD in services:
# enable gripper controller
self.cmdr.append(GripperController())
if ARM_CMD in services:
# enable arm controller
self.cmdr.append(ArmController())
if ODOM_LSTN in services:
# enable default odometry listener
self.lstnr.append(OdometryListener())
if IR_LSTN in services:
# enable default ir listener
self.lstnr.append(IrListener())
if LIDAR_LSTN in services:
# enable default lidar listener
self.using_lidar = True
self.lstnr.append(LidarListener())
if GRIPPER_LSTN in services:
# enable default gripper has pot detection
self.lstnr.append(GripperListener())
if NEAREST_OBS in service:
# enable nearest obsticale location listener
self.using_lidar = True
self.lstnr.append(LidarNObsListener())
if ODOM_GYRO_LSTN in services:
# enable odometry with gryro correction listener
self.lstnr.append(OdometryGyroListener())
self.capture_buffer = ()
self.probe_names = {}
self.probe_defs = {}
......@@ -76,38 +39,6 @@ class Client:
except Exception as e:
raise e
def start(self, silent=True):
# start up ROS bridge mode for robot
try:
# this will prevent the robot from emiting beeps
if silent:
self.write((BEEP, 0))
# start the robot
self.write((FAKE_START, 1))
# this will start collection with the lidar sensor
if self.using_lidar:
self.write((LIDAR_STANDBY, 0))
self.enable_services()
except Exception as e:
raise e
def stop(self):
# end everything that is running
try:
# 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)])
print "virtual e-stop triggered"
time.sleep(1)
self.write([(2426, 0), (2419, 0)])
print str(self.hostname) + " has stopped all running processes"
except Exception as e:
raise e
def connect(self, host, port=4400):
# Makes a connection to the robot.
# performs an initialization handshake to get version, tick rate and probes
......@@ -169,22 +100,6 @@ class Client:
print "Client Disconnet Failed"
raise e
## ----- Service Methods ----- ##
def write(self, msgs):
# write a set of values to a set of probes
try:
v = ""
if type(msgs) != tuple:
msgs = ((msgs),)
for (probe, value) in msgs:
probe_id = self.lookup_probe_id(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))
except Exception as e:
raise e
## ----- Probe Methods ----- ##
......@@ -266,17 +181,6 @@ class Client:
except Exception as e:
raise e
def include_probes(self, probes):
# adds a probe code or collection of probe codes to the collection of probes
try:
if type(probes) != tuple:
probes = ((probes),)
for probe in probes:
if probe not in self.probe_list:
self.probe_list.append(probe)
except Exception as e:
raise e
def encode_probe_value(self, probe_type, value):
# encode as tlv msg
if self.type_table[probe_type][0] == "tlv":
......@@ -301,11 +205,35 @@ class Client:
return data
except:
raise RuntimeError("struct unpack failed")
def start_probes(self):
# start capturing information from a probe
self.capturing = True
self.send_message(make_tlv(MP_TLV_START_PROBES))
def stop_probes(self):
# stop capturing information from a probe
self.send_message(make_tlv(MP_TLV_STOP_PROBES))
self.capturing = False
def write_probe(self, probe, value):
# write a vale to a probe
probe_id = self.lookup_probe_id(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))
def write_probes(self, probevals):
# write a set of values to a set of probes
v = ""
def init_probes(self):
# enables all the probes on the start list
self.enable_probes(tuple(self.probe_list))
# self.probe_list = []
for (probe, value) in probevals:
probe_id = self.lookup_probe_id(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))
## ---- Message Handler Methods ---- ##
......@@ -363,7 +291,7 @@ class Client:
(t, l, v) = self.listener_q.get()
return (t, l, v)
## ----- Data Capture Methods ----- ##
## ----- Data Capture Methods Not currently used in bridging function ----- ##
def capture_buffer_len(self):
# length of capture buffer
......@@ -381,11 +309,9 @@ class Client:
self.capture_buffer = ()
self.capture_buffer_lock.release()
self.capturing = True
self.send_message(make_tlv(MP_TLV_START_PROBES))
self.start_probes()
time.sleep(t) # let the listener thread do the capturing
self.send_message(make_tlv(MP_TLV_STOP_PROBES))
self.capturing = False
self.stop_probes()
def capture_data(self, data):
# Add the data an item in the caputre buffer.
......@@ -446,52 +372,13 @@ class Client:
self.capture_buffer_lock.release()
return captured_data_array
def show_capture(self, interval=None):
# get data from the capture buffer and print it to console
if interval == None:
interval = 1.0 / self.hz
step = int(round(self.hz * interval))
self.capture_buffer_lock.acquire()
for i in range(0, len(self.capture_buffer), step):
item = self.capture_buffer[i]
if len(item) == 0:
print "empty capture buffer item"
continue
(t, l, v, item) = next_tlv(item)
if t != MP_TLV_CURRENT_TICK:
print "bad capture item, first element not tick"
continue
(val,) = struct.unpack("<I", v)
print "tick =", val,
print ("Probes: ", len(self.probe_defs))
while len(item):
(t, l, v, item) = next_tlv(item)
if self.probe_defs[t] == None:
pass
name = self.probe_defs[t][0]
type = self.probe_defs[t][1]
val = self.decode_probe_data(type, v)
print name, "=", val,
print ""
self.capture_buffer_lock.release()
## ----- Listener Methods ----- ##
def start_listener(self):
def start_listener(self, n=10):
# start listener conenction
self.listener_pipe = os.pipe()
self.listener_q = Queue.Queue()
self.listener_q = Queue.Queue(maxsize=n)
self.listener = self.listen(self.listener_q, self)
self.listener.start()
......
class listen(threading.Thread):
# listener subclass for getting data from robot
def __init__(self, q, mp):
# listener class for getting data from robot
def __init__(self, q, client):
threading.Thread.__init__(self)
self.q = q
self.mp = mp
self.callback = None
self.setDaemon(True)
def addMsgCallback(callbackFcn):
# adds a function that is called whenever a message is recived by the queue
self.callback = callbackFcn
def run(self):
# runs a listener thread, reading in data and placing it in a queue
while True:
# Wait for incoming data from the RCP or for the listener
# pipe to close, indicating that we are
(r, w, x) = select.select((self.mp.s, self.mp.listener_pipe[0]), (), ())
if self.mp.listener_pipe[0] in r:
# listener pipe signaled the thread to exit
os.close(self.mp.listener_pipe[0])
return
if self.mp.s in r:
(t, l, v) = self.mp.receive_next_message()
if not self.mp.do_async_message(t, l, v):
self.q.put((t, l, v))
# time.sleep(0.1)
try:
while True:
# Wait for incoming data from the RCP or for the listener
# pipe to close, indicating that we are
(r, w, x) = select.select((self.client.s, self.client.listener_pipe[0]), (), ())
if self.client.listener_pipe[0] in r:
# listener pipe signaled the thread to exit
os.close(self.client.listener_pipe[0])
return
if self.client.s in r:
(t, l, v) = self.client.receive_next_message()
if not self.client.do_async_message(t, l, v):
self.q.put((t, l, v))
try:
if self.callback == None:
raise NameError("No Callback has been defined")
self.callback(t,v)
except NameError:
pass
# time.sleep(0.1)
except Exception as e:
raise e
class Reader():
# reader class for getting data from the robot and passing to ROS
def __init__(self):
pass
def run(self):
# runs a listener thread, reading in data and placing it in a queue
try:
while True:
# Wait for incoming data from the RCP or for the listener
# pipe to close, indicating that we are
(r, w, x) = select.select((self.client.s, self.client.listener_pipe[0]), (), ())
if self.client.listener_pipe[0] in r:
# listener pipe signaled the thread to exit
os.close(self.client.listener_pipe[0])
return
if self.client.s in r:
(t, l, v) = self.client.receive_next_message()
if not self.client.do_async_message(t, l, v):
self.q.put((t, l, v))
try:
if self.callback == None:
raise NameError("No Callback has been defined")
self.callback(t,v)
except NameError:
pass
# time.sleep(0.1)
except Exception as e:
raise e
class Reader():
# reader class for getting data from the robot and passing to ROS
def __init__(self):
pass
def run(self):
# runs a listener thread, reading in data and placing it in a queue
try:
while True:
# Wait for incoming data from the RCP or for the listener
# pipe to close, indicating that we are
(r, w, x) = select.select((self.client.s, self.client.listener_pipe[0]), (), ())
if self.client.listener_pipe[0] in r:
# listener pipe signaled the thread to exit
os.close(self.client.listener_pipe[0])
return
if self.client.s in r:
(t, l, v) = self.client.receive_next_message()
if not self.client.do_async_message(t, l, v):
self.q.put((t, l, v))
try:
if self.callback == None:
raise NameError("No Callback has been defined")
self.callback(t,v)
except NameError:
pass
# time.sleep(0.1)
except Exception as e:
raise e
......@@ -3,10 +3,11 @@ from std_msgs.msg import Bool
import rospy
import sys, os
SCRIPTS_PATH = os.path.abspath(os.path.join(sys.path[0], "../../../resources"))
SCRIPTS_PATH = os.path.abspath(os.path.join(sys.path[0], "../../"))
sys.path.append(SCRIPTS_PATH)
from getter import Getter
from utils import resource_name
from libs.Resources import Getter
from libs.Resources.utils import *
class Gripper(Getter):
......
......@@ -3,7 +3,7 @@ from std_msgs.msg import Float64
import sys, os
import rospy
SCRIPTS_PATH = os.path.abspath(os.path.join(sys.path[0], "../../../resources"))
SCRIPTS_PATH = os.path.abspath(os.path.join(sys.path[0], "../"))
sys.path.append(SCRIPTS_PATH)
from ir import IrCamera
......@@ -101,11 +101,11 @@ class SensorHub:
# probe id 3199: vis.lineSensor.line.now.pt.y type float length 4
# probe id 3182: vis.lineSensor.cand.local.pt.x type float length 4
# probe id 3183: vis.lineSensor.cand.local.pt.y type float length 4
self.sick = SickLineSensor(
("vis.lineSensor.line.now.pt.x", "vis.lineSensor.line.now.pt.y"),
(3198, 3199),
self.mp,
)
#self.sick = SickLineSensor(
# ("vis.lineSensor.line.now.pt.x", "vis.lineSensor.line.now.pt.y"),
# (3198, 3199),
# self.mp,
#)
# initialize the nearest obstacle detection of the preprocessed laser data
# codes give: x, y, and distance
......@@ -200,7 +200,7 @@ class SensorHub:
# start the lidar camera
self.lidar.init(ros, hostname)
self.sick.init(ros, hostname)
#self.sick.init(ros, hostname)
self.forwardObs.init(ros, hostname)
# start the odometery
......@@ -236,7 +236,7 @@ class SensorHub:
# run the lidar camera
self.lidar.run(self.queue, current_time)
self.sick.run(self.queue, current_time)
#self.sick.run(self.queue, current_time)
self.forwardObs.run(self.queue, current_time)
# run the odometry
......
......@@ -3,10 +3,11 @@ from std_msgs.msg import Float32
import rospy
import sys, os
SCRIPTS_PATH = os.path.abspath(os.path.join(sys.path[0], "../../../resources"))
SCRIPTS_PATH = os.path.abspath(os.path.join(sys.path[0], "../../"))
sys.path.append(SCRIPTS_PATH)
from getter import Getter
from utils import resource_name
from libs.Resources import Getter
from libs.Resources.utils import *
class IrCamera(Getter):
......
......@@ -14,10 +14,11 @@ import sys, os
import numpy as np
import traceback
SCRIPTS_PATH = os.path.abspath(os.path.join(sys.path[0], "../../../resources"))
SCRIPTS_PATH = os.path.abspath(os.path.join(sys.path[0], "../../"))
sys.path.append(SCRIPTS_PATH)
from getter import Getter
from utils import resource_name
from libs.Resources import Getter
from libs.Resources.utils import *
class LidarSensor(Getter):
......@@ -43,6 +44,7 @@ class LidarSensor(Getter):
try:
data = q[self.name]
#print data
if type(data) != str:
raise Exception("Data not in correct format!")
# get the data from the message
......@@ -52,28 +54,34 @@ class LidarSensor(Getter):
# create x and y arrays from the base16 string of data
strarr = data.split(":")
dat = []
obj = []
for i in range(len(strarr)):
if strarr[i] != "":
dat.append(int(strarr[i], 16))
datStr = ""
for elem in strarr:
if elem != "":
datStr += chr(int(elem, 16))
dat = [struct.unpack("<f", datStr[i : i + 4]) for i in range(0, len(datStr), 4)]
#elem_arr = np.zeros((361,3),dtype=float)
elem_arr = []
for i in range(0, len(dat), 6):
if len(dat[i:]) < 6:
j = 0
for i in range(0, len(dat), 3):
if len(dat[i:]) < 3:
break
tx = struct.pack("<HH", dat[i],dat[i+1])
ty = struct.pack("<HH", dat[i+2],dat[i+3])
x = struct.unpack("<f", tx)
y = struct.unpack("<f", ty)
z = 0.05
elem_arr.append([x, y, z])
#if j > 360:
# break
elem_arr.append([dat[i],dat[i+1],0.05])
z = 0.0
#print "x = %f, y = %f" % (x,y)
#elem_arr[j,0] = dat[i]
#elem_arr[j,1] = dat[i+1]
#j = j + 1
elem_arr = np.array(elem_arr)
header = Header()
header.stamp = current_time
header.frame_id = self.hostname
msg = pc2.create_cloud_xyz32(header, np.array(elem_arr))
msg = pc2.create_cloud_xyz32(header, elem_arr)
# publish data
Getter.run(self, msg)
......@@ -177,6 +185,7 @@ class SickLineSensor(Getter):
def run(self, q, current_time):
## handles the publishing of data, should be run at an acceptable update interval
# get data from queue
print "WHY IS THIS RUNNING!!!"
try:
x = 0
......
......@@ -13,10 +13,11 @@ import rospy
import tf
import sys, os
SCRIPTS_PATH = os.path.abspath(os.path.join(sys.path[0], "../../../resources"))
SCRIPTS_PATH = os.path.abspath(os.path.join(sys.path[0], "../../"))
sys.path.append(SCRIPTS_PATH)
from getter import Getter
from utils import resource_name
from libs.Resources import Getter
from libs.Resources.utils import *
class Odometer(Getter):
......
File mode changed from 100644 to 100755
......@@ -59,6 +59,79 @@ class HVClient(Client):
# enable odometry with gryro correction listener
self.lstnr.append(OdometryGyroListener())
Client(services)
Client(self)
except Exception as e:
raise e
def start(self, silent=True):
# start up ROS bridge mode for robot
try:
# this will prevent the robot from emiting beeps
if silent:
self.write((BEEP, 0))
# start the robot
self.write((FAKE_START, 1))