Commit 17fa7e54 authored by Nicholas Shindler's avatar Nicholas Shindler
Browse files

read incoming probe msgs directly to bridge

parent db4e2eb1
......@@ -44,6 +44,8 @@ class Client:
self.listener = None
self.connect(host)
# wait for the connection to be fully made
time.sleep(3)
def connect(self, host, port=4400):
# Makes a connection to the robot.
......@@ -412,10 +414,6 @@ 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):
......
......@@ -20,10 +20,6 @@ class listen(threading.Thread):
self.callback = None
self.setDaemon(True)
def addMsgCallback(self, 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
try:
......@@ -39,14 +35,10 @@ class listen(threading.Thread):
return
if self.client.s in r:
(t, l, v) = self.client.receive_next_message()
# if not self.client.read_async_message(t,l,v):
# TODO: might need to change name rather than doing a method overload
if not self.client.do_async_message(t, l, v):
self.q.put((t, l, v))
try:
if self.callback != None:
self.callback(t, v)
except Exception as e:
rospy.logerr(e)
# time.sleep(0.1)
except Exception as e:
rospy.logwarn(e)
raise e
......@@ -93,16 +93,17 @@ class HVClient(Client):
# start the robot
Client.write_probe(self, START, 1)
time.sleep(2)
# this will start collection with the lidar sensor
if self.using_lidar:
Client.write_probe(self, LIDAR_STANDBY, 0)
time.sleep(1)
self.enable_services()
time.sleep(1)
Client.addListenerCallback(self, self.read)
Client.start_probes(self)
# Client.start_probes(self)
except Exception as e:
raise e
......@@ -111,6 +112,7 @@ class HVClient(Client):
try:
# sets the start to FALSE. this 'stops' the robot.
# sets flag pulled to true
Client.stop_probes(self)
Client.write_probes(self, [(ESTOP, 1), (START, 0), (FLAG_PULL, 1)])
print "virtual e-stop triggered"
time.sleep(1)
......@@ -133,7 +135,8 @@ class HVClient(Client):
probes = get_probe_arr(
set([]), map((lambda a: set(a.get_probe_list())), self.lstnr)
)
print ("Probes to enable: ")
print (probes)
Client.enable_probes(self, probes)
except Exception as e:
raise e
......@@ -161,17 +164,39 @@ class HVClient(Client):
rospy.logwarn(e)
# raise e
def read(self, t, v):
def do_async_message(self, t, l, v):
# async msg handler
if t == MP_TLV_PROBE_DATA:
self.read_probe_msg(v)
return True
elif t == MP_TLV_DEBUG_MESSAGE:
Client.do_debug_message(self, v)
return True
elif t == MP_TLV_MISSED_DATA:
print ("Missed %d probe messages" % struct.unpack("<I", v))
return True
elif t == MP_TLV_MISSED_DEBUG_MESSAGES:
print ("Missed %d debug messages" % struct.unpack("<I", v))
return True
else:
return False # not an asynchronous message
def read_probe_msg(self, data):
# pass data to the correct child class
try:
(pname, ptype) = Client.lookup_probe(self, t)
val = Client.decode_probe_data(self, ptype, v)
print ("name: " + pname + " val: " + val)
[serv.add(pname, val) for serv in self.lstnr]
print ("CALLED!!! the read msg\n\n\n")
while len(item):
(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]):
val = Client.decode_probe_data(self, probe_type, v)
print ("New Message[name: " + probe_name + " val: " + val + "]")
[serv.add(probe_name, val) for serv in self.lstnr]
else:
rospy.logwarn("Probe info not needed: %s" % probe_name)
except Exception as e:
rospy.logwarn(e)
raise e
def publish(self):
......
......@@ -161,15 +161,11 @@ if __name__ == "__main__":
hv_bot = HVClient(host_name, services)
# start the connection to the robot
hv_bot.start()
time.sleep(2)
# handle closing everything nicely when ros shutsdown
rospy.on_shutdown(hv_bot.stop)
# enable the listener services
hv_bot.enable_services()
# start the connection to the robot
hv_bot.start()
time.sleep(1)
# set rate based on the mindprobe refresh rate.
......@@ -186,6 +182,7 @@ if __name__ == "__main__":
)
while not rospy.is_shutdown():
# write any messages from ROS to the robot
# hv_bot.capture(1 / 200)
hv_bot.write()
hv_bot.publish()
......
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