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

fixed errors with reader/writer

parent 3538da8b
......@@ -17,7 +17,7 @@ class ArmController(Writer):
def __init__(self, hostname):
# Create writer for this probe
# manip.armRequestedTargetPositionInPercent
Writer.__init__(self, ["manip.armTargetPositionInPercent"])
Writer.__init__(self, ["manip.armRequestedTargetPositionInPercent"])
# runs a subscriber for setting the arm position.
rospy.Subscriber("/" + hostname + "/set_arm_pos", Float32, self.callback)
......
......@@ -17,7 +17,7 @@ class GripperController(Writer):
def __init__(self, hostname):
# Create writer for this probe
# manip.gripperRequestedTargetPositionInPercent
Writer.__init__(self, ["manip.gripperTargetPositionInPercent"])
Writer.__init__(self, ["manip.gripperRequestedTargetPositionInPercent"])
# intializes the mp connection and runs a subscriber for setting the gripper position.
rospy.Subscriber("/" + hostname + "/set_gripper_pos", Float32, self.callback)
......
......@@ -94,9 +94,6 @@ class Client:
def disconnect(self):
try:
# call e-stop to and all functionality before breaking connection
self.stop()
# end the connection
self.stop_listener()
self.s.close()
......@@ -166,7 +163,9 @@ class Client:
elif type(probe) == int:
probe_id = probe
else:
raise ValueError("probe must be int or string")
raise ValueError(
"probe must be int or string, probe is %s" % type(probe)
)
return probe_id
except Exception as e:
......
......@@ -42,11 +42,10 @@ class listen(threading.Thread):
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
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)
......
......@@ -2,6 +2,10 @@ class Reader:
# reader class for getting data from the robot and passing to ROS
def __init__(self, p):
# constructor for settings class
if type(p) != tuple and type(p) != list and type(p) != str:
raise TypeError(
"probes should be an itterable list/tupple or a string not %s" % type(p)
)
self.pub = None
self.probes = p
self.queue = {}
......@@ -15,6 +19,8 @@ class Reader:
def add(self, name, val):
# add data piece
try:
if type(self.probes) == str or name == self.probes:
self.queue[name] = val
if name in self.probes:
self.queue[name] = val
except Exception as e:
......@@ -22,8 +28,15 @@ class Reader:
def needs(self, name):
# check if name is used by this class
return True if name in self.probes else False
return name == self.probes if type(self.probes) == str else name in self.probes
def get_probes(self):
# return an array of all probes
return self.probes if len(self.probes) > 1 else self.probes[0]
return (
self.probes
if len(self.probes) > 1 or type(self.probes) == str
else self.probes[0]
)
def get_probe_list(self):
return self.probes if type(self.probes) != str else [self.probes]
......@@ -5,6 +5,10 @@ class Writer:
def __init__(self, p):
# constructor for settings class
if type(p) != tuple and type(p) != list and type(p) != str:
raise TypeError(
"probes should be an itterable list/tupple or a string not %s" % type(p)
)
self.probes = p
def get_msgs(self):
......@@ -19,10 +23,10 @@ class Writer:
def callback(self, msg):
# subscriber callback function
if len(self.probes) == 1:
if type(self.probes) == str:
# if the message is a single item value pair
self.msgs = [(self.probes, msg)]
elif len(msg) > 1:
elif type(msg) == list:
# handle an array of messages
# each message coresponds to a probe (1 to 1)
if len(msg) != len(self.probes):
......
......@@ -111,14 +111,16 @@ class HVClient(Client):
try:
# sets the start to FALSE. this 'stops' the robot.
# sets flag pulled to true
# TODO: switch codes for enums
Client.write_probes(self, [(ESTOP, 1), (START, 0), (FLAG_PULL, 1)])
print "virtual e-stop triggered"
time.sleep(1)
Client.write_probes(self, [(ESTOP, 0), (FLAG_PULL, 0)])
print str(self.hostname) + " has stopped all running processes"
except Exception as e:
rospy.logerr(e)
raise e
finally:
Client.disconnect(self)
def enable_services(self):
# take all probes that need to be listened for and enable them
......@@ -128,7 +130,9 @@ class HVClient(Client):
if len(b) > 1
else tuple(a.union(b[0]))
)
probes = get_probe_arr(set([]), map((lambda a: set(a.probes)), self.lstnr))
probes = get_probe_arr(
set([]), map((lambda a: set(a.get_probe_list())), self.lstnr)
)
Client.enable_probes(self, probes)
except Exception as e:
......@@ -144,9 +148,8 @@ class HVClient(Client):
v = ""
if len(msgs) == 0:
return
print msgs
print len(msgs)
for (probe, value) in msgs:
print (probe, value)
probe_id = Client.lookup_probe_id(self, probe)
probe_type = Client.lookup_probe_type(self, probe_id)
......@@ -163,10 +166,12 @@ class HVClient(Client):
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]
except Exception as e:
rospy.logwarn(e)
raise e
def publish(self):
......
......@@ -94,7 +94,7 @@ if __name__ == "__main__":
# set the hostname based on the argument inputed by the user (default: hai-1095.local)
host_name = args.hostname
print(args)
services = []
if args.all:
# connect all sensors and controllers
......@@ -112,7 +112,7 @@ if __name__ == "__main__":
]
else:
if args.sensor_flag:
print("using all sensors")
# add all sensors
services += [
ODOM_LSTN,
IR_LSTN,
......@@ -122,7 +122,7 @@ if __name__ == "__main__":
ODOM_GYRO_LSTN,
]
elif args.sensors != None:
print(args.sensors)
# add selected sensors
if 1 in args.sensors:
services.append(LIDAR_LSTN)
if 2 in args.sensors:
......@@ -136,10 +136,10 @@ if __name__ == "__main__":
if 6 in args.sensors:
services.append(NEAREST_OBS)
if args.controller_flag:
print("using all controllers")
# add all controllers
services += [DRIVE_CMD, GRIPPER_CMD, ARM_CMD]
elif args.controllers != None:
print(args.controllers)
# add only the selected controllers
if 3 in args.sensors:
services.append(DRIVE_CMD)
if 2 in args.controllers:
......@@ -147,7 +147,7 @@ if __name__ == "__main__":
if 1 in args.controllers:
services.append(ARM_CMD)
if "all" in args.services:
print("services!")
# add all services
services += [FOLLOW_SRV, PICK_SRV, AVOID_SRV]
elif args.services != []:
if "follow" in args.services:
......@@ -157,17 +157,16 @@ if __name__ == "__main__":
if "avoid" in args.services:
services.append(AVOID_SRV)
print("connecting to {}").format(host_name)
rospy.loginfo("Connecting to %s", host_name)
print(services)
hv_bot = HVClient(host_name, services)
# start the connection to the robot
hv_bot.start()
time.sleep(1)
time.sleep(2)
# handle closing everything nicely when ros shutsdown
rospy.on_shutdown(lambda: hv_bot.stop())
rospy.on_shutdown(hv_bot.stop)
# enable the listener services
hv_bot.enable_services()
......@@ -182,7 +181,9 @@ if __name__ == "__main__":
# Start the ROS main loop
rate = rospy.Rate(hz)
print("starting publishing data from " + host_name + " at " + str(hz) + "hz")
rospy.loginfo(
"starting publishing data from " + host_name + " at " + str(hz) + "hz"
)
while not rospy.is_shutdown():
# write any messages from ROS to the robot
hv_bot.write()
......@@ -191,13 +192,12 @@ if __name__ == "__main__":
rate.sleep()
except rospy.ROSInterruptException:
print("ROS interupt called -- Ending Processes")
pass
rospy.logerr("ROS interupt called -- Ending Processes")
except Exception as e:
# general error handling
err = traceback.format_exc()
rospy.logerr(e)
print(err)
finally:
# NOTE: this may not actually be called in most cases, and should be handled in the on shutdown function
# mp.disconnect()
print("GOODBYE")
# last code to run before everything closes
rospy.loginfo("GOODBYE")
......@@ -5,7 +5,7 @@ from __future__ import print_function
# import roslib; roslib.load_manifest('teleop_twist_keyboard')
import rospy
from geometry_msgs.msg import Twist
from std_msgs.msg import Float64
from std_msgs.msg import Float32, Float64
import sys, os, select, termios, tty, math
......@@ -155,10 +155,10 @@ if __name__ == "__main__":
"/" + resource_name(host_name) + "/cmd_vel", Twist, queue_size=1
)
pub_gripper = rospy.Publisher(
"/" + resource_name(host_name) + "/set_gripper_pos", Float64, queue_size=1
"/" + resource_name(host_name) + "/set_gripper_pos", Float32, queue_size=1
)
pub_arm = rospy.Publisher(
"/" + resource_name(host_name) + "/set_arm_pos", Float64, queue_size=1
"/" + resource_name(host_name) + "/set_arm_pos", Float32, queue_size=1
)
# init ros
......@@ -185,8 +185,8 @@ if __name__ == "__main__":
print(pos(arm, gripper))
# Create the msgs for arm and gripper
arm_msg = Float64()
gripper_msg = Float64()
arm_msg = Float32()
gripper_msg = Float32()
rate = rospy.Rate(100) # set update rate
# Start the ROS main loop
......
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