Commit 11bb9f82 authored by Nicholas Shindler's avatar Nicholas Shindler
Browse files

add in more effective error handling

parent 366df144
......@@ -52,4 +52,5 @@ this is for harvest vehicle robots, property of a private company and therefore
|--- hv_bridge: lauch the bridge
|--- harvey: lauch the bridge + the demo application
|--- hv_teleop: lauch the bridge + the teleop node
*mindprobe_scripts*: HAi scripts for mindprobe server and client.
```
......@@ -42,6 +42,8 @@ class Client:
self.capturing = False
self.listener_pipe = None
self.listener = None
self.found_probes=False
self.listener_closed = True
self.connect(host)
# wait for the connection to be fully made
......@@ -81,9 +83,11 @@ class Client:
# Discover the probes:
time.sleep(0.5)
while len(self.probe_defs.keys()) < 3900:
while len(self.probe_defs.keys()) < TOTAL_PROBES:
self.discover_probes()
time.sleep(0.1)
# toggle state to record that the probes have been looked up and stored
self.found_probes=False
print "Protocol version = 0x%04x, sample rate is %d Hz, %d probes" % (
self.version,
......@@ -98,12 +102,22 @@ class Client:
try:
# end the connection
self.stop_listener()
while(not self.listener_closed):
time.sleep(0.5)
self.s.close()
print ("mindprobe has disconnected")
return True
print "Client has disconnected"
except Exception as e:
print "Client Disconnet Failed"
raise e
def reconnect(self):
try:
# reconnect to the server
self.stop_listener()
time.sleep(2)
self.start_listener()
except Exception as e:
raise e
## ----- Probe Methods ----- ##
......@@ -185,8 +199,13 @@ class Client:
def lookup_probe(self, t):
# get the probe info
try:
if not self.found_probes:
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)
if (t not in self.probe_defs):
raise LookupError("Probe %i not found in probe defs" % t)
return (self.probe_defs[t][0], self.probe_defs[t][1])
except Exception as e:
raise e
......@@ -406,13 +425,20 @@ class Client:
self.listener = listen(self.listener_q, self)
self.listener.start()
self.listener_closed = False
def stop_listener(self):
# close listener connection
# TODO: why does this line fail?
# os.write(self.listener_pipe[1], 'bye')
os.close(self.listener_pipe[1])
self.listener.join()
try:
os.write(self.listener_pipe[1], 'bye')
os.close(self.listener_pipe[1])
except Exception as e:
print "listener pipe not closing"
pass
finally:
self.listener.join()
self.listener_closed = True
## ----- Script Methods ----- ##
......
from enum import Enum
TOTAL_PROBES = 3900
###
# Type names for probe types (t in tlv)
###
......
......@@ -9,6 +9,9 @@ import os
import sys
import subprocess
import rospy
import traceback
from socket import error as SocketError
class listen(threading.Thread):
......@@ -39,6 +42,15 @@ class listen(threading.Thread):
# 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))
except Exception as e:
except OSError as e:
rospy.logerr("SOCKET ERROR")
rospy.logwarn(e)
raise e
if e.errno != errno.ECONNRESET:
raise
client.reconnect()
except Exception as e:
rospy.logerr("NON-SOCKET ERROR")
err = traceback.format_exc()
rospy.logerr(e)
rospy.loginfo(err)
#raise
......@@ -40,7 +40,7 @@ class PickTargetService(Writer):
def set_pick_target(self, req):
# set values for target
Writer.callback(self, (0, req.x, req.y))
Writer.callback(self, [0, req.x, req.y])
return "suceess: set target to (" + str(req.x) + "," + str(req.y) + ")"
......
......@@ -26,6 +26,7 @@ from libs import *
class HVClient(Client):
stopped = False
cmdr = []
lstnr = []
srvs = []
......@@ -101,13 +102,15 @@ class HVClient(Client):
Client.write_probe(self, LIDAR_STANDBY, 0)
time.sleep(1)
self.enable_services()
# if data needs to be collected from the robot, enable the probes
if(len(self.lstnr) != 0):
self.enable_services()
time.sleep(1)
Client.start_probes(self)
except Exception as e:
err = traceback.format_exc()
rospy.logerr(e)
rospy.logerr(err)
raise e
def stop(self):
......@@ -118,10 +121,11 @@ class HVClient(Client):
# 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)])
Client.write_probes(self, [(ESTOP, 1), (FLAG_PULL, 1)])
print "virtual e-stop triggered"
time.sleep(1)
Client.write_probes(self, [(ESTOP, 0), (FLAG_PULL, 0)])
Client.write_probe(self, START, 0)
print str(self.hostname) + " has stopped all running processes"
except Exception as e:
rospy.logerr(e)
......@@ -133,10 +137,18 @@ class HVClient(Client):
self.cmdr = []
self.lstnr = []
self.srvs = []
self.stopped = True
def restart(self):
# stop everything and restart the connection
self.stop()
time.sleep(5)
self.start()
def enable_services(self):
# take all probes that need to be listened for and enable them
try:
# this will throw an error if the lstnr list is empty
get_probe_arr = (
lambda a, b: get_probe_arr(a.union(b[0]), b[1:])
if len(b) > 1
......@@ -157,14 +169,21 @@ 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 = []
# collect all subscription messages to write to robot
msgs = get_msg_arr([], map((lambda a: a.get_msgs()), self.cmdr))
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
msgs += get_msg_arr([], map((lambda a: a.get_msgs()), self.srvs))
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:
if(type(value) == tuple or type(value) == list):
#raise TypeError("message value incorrectly formatted")
rospy.logerr("Probe [%s] has an incorrectly formatted value", probe)
continue
probe_id = Client.lookup_probe_id(self, probe)
probe_type = Client.lookup_probe_type(self, probe_id)
......@@ -173,8 +192,7 @@ class HVClient(Client):
)
Client.send_message(self, make_tlv(MP_TLV_WRITE_PROBES, v))
except Exception as e:
rospy.logwarn(e)
# raise e
raise e
def do_async_message(self, t, l, v):
# async msg handler
......@@ -209,7 +227,12 @@ class HVClient(Client):
# rospy.loginfo("Probe info not needed: %s" % probe_name)
pass
except RuntimeWarning as e:
#rospy.logwarn(e)
pass
except Exception as e:
err = traceback.format_exc()
rospy.logwarn(err)
raise e
def publish(self):
......
......@@ -22,6 +22,7 @@ if __name__ == "__main__":
# This node must be run to allow the connection between ROS and the HarvestAI bots over
# the Mindprobe protocol
###
hv_bot = None
try:
# Init the connection with the ROS system
rospy.init_node("hai_main_node", anonymous=True)
......@@ -31,8 +32,11 @@ if __name__ == "__main__":
)
# Add the arguments to handle setting various sensor and controller connections
parser.add_argument(
"hostname",
"-n",
"--name",
"--hostname",
metavar="hostname",
dest="hostname",
type=str,
default="hai-1095.local",
nargs="?",
......@@ -189,11 +193,25 @@ if __name__ == "__main__":
except rospy.ROSInterruptException:
rospy.logerr("ROS interupt called -- Ending Processes")
except OSError as e:
rospy.logerr("Connection failed!")
err = traceback.format_exc()
rospy.logerr(e)
rospy.loginfo(err)
if e.errno == errno.ECONNRESET:
rospy.logerr("CONNECTION RESET BY PEER")
if(hv_bot != None and not hv_bot.stopped):
rospy.loginfo("Reconnecting to Server")
hv_bot.reconnect()
except Exception as e:
# general error handling
err = traceback.format_exc()
rospy.logerr(e)
print(err)
rospy.loginfo(err)
print("the type should be: %s" % type(e))
pass
finally:
# last code to run before everything closes
if(hv_bot != None and not hv_bot.stopped):
hv_bot.stop()
rospy.loginfo("GOODBYE")
<launch>
<node name="main_node.py" pkg="hv_bridge" type="bridge" args="--all" />
<node name="harvey_node" pkg="harvey" type="talker" />
<node name="hv_bridge" pkg="hv_bridge" type="main_node.py" args="--all --hostname hai-1095.local" output="screen" required="true"/>
<node name="harvey" pkg="harvey" type="harvey_node" output="screen" required="true"/>
</launch>
<launch>
<node name="hv_bridge" pkg="hv_bridge" type="main_node.py" args=" --all hai-1095.local" />
<node name="hv_bridge" pkg="hv_bridge" type="main_node.py" args=" --all --hostname hai-1095.local" output="screen" required="true"/>
</launch>
<launch>
<node name="main_node.py" pkg="hv_bridge" type="bridge" args="--all-controllers" />
<node name="teleop_node.py" pkg="hv_teleop" type="talker" />
<node name="hv_bridge" pkg="hv_bridge" type="main_node.py" args="--all-controllers --hostname hai-1095.local" output="log" required="true"/>
<node name="teleop" pkg="hv_teleop" type="teleop_node.py" args="--hostname hai-1095.local" output="screen" respawn="false"/>
</launch>
......@@ -8,16 +8,29 @@ from geometry_msgs.msg import Twist
from std_msgs.msg import Float32, Float64
import sys, os, select, termios, tty, math
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)
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,"%"))
file.flush()
show(0)
for i, item in enumerate(it):
yield item
show(i+1)
file.write("\n")
file.flush()
# variables
host_name = "hai-1095.local"
# variables
pub_drive = None
pub_gripper = None
pub_arm = None
......@@ -148,17 +161,52 @@ if __name__ == "__main__":
# uses keyboard commands to publish velocity, gipper and arm values with ROS
# should be run allong with services node
###
parser = argparse.ArgumentParser(
description="ROS Connection Application for comunication and control of the HV Robots"
)
# Add the arguments to handle setting various sensor and controller connections
parser.add_argument(
"-n",
"--name",
"--hostname",
metavar="hostname",
dest="hostname",
type=str,
default="hai-1095.local",
nargs="?",
help="The name of the robot, generally of the form hai-####.local (default: hai-1095.local",
)
parser.add_argument(
"sys",
type=str,
nargs="*",
help="System included variables from launch file (not set bu user)",
)
args = parser.parse_args()
# set the hostname based on the argument inputed by the user (default: hai-1095.local)
host_name = args.hostname
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(
"/" + resource_name(host_name) + "/cmd_vel", Twist, queue_size=1
"/" + name + "/cmd_vel", Twist, queue_size=1
)
pub_gripper = rospy.Publisher(
"/" + resource_name(host_name) + "/set_gripper_pos", Float32, queue_size=1
"/" + name + "/set_gripper_pos", Float32, queue_size=1
)
pub_arm = rospy.Publisher(
"/" + resource_name(host_name) + "/set_arm_pos", Float32, queue_size=1
"/" + name + "/set_arm_pos", Float32, queue_size=1
)
# init ros
......
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