fixed errors with reader/writer

hv_bridge/libs/Controllers/arm.py

@@ -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)
 

hv_bridge/libs/Controllers/gripper.py

@@ -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)
 

hv_bridge/libs/Resources/client.py

@@ -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:

hv_bridge/libs/Resources/listener.py

@@ -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)

hv_bridge/libs/Resources/reader.py

@@ -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]

hv_bridge/libs/Resources/writer.py

@@ -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):

hv_bridge/scripts/hv_client.py

@@ -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):

hv_bridge/scripts/main_node.py

@@ -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")

hv_teleop/scripts/teleop_node.py

@@ -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