more cleaning up code

hv_bridge/libs/Controllers/arm.py

@@ -14,12 +14,10 @@ class ArmController(Writer):
     # Controller for the robot arm
     # Extends the Setter class
     ###
-    def __init__(self):
+    def __init__(self, hostname):
         # Create writer for this probe
         # manip.armRequestedTargetPositionInPercent
         Writer.__init__(self, ["manip.armTargetPositionInPercent"])
-
-    def run(self, hostname):
         # runs a subscriber for setting the arm position.
         rospy.Subscriber("/" + hostname + "/set_arm_pos", Float32, self.callback)
 

hv_bridge/libs/Controllers/drive.py

@@ -18,12 +18,11 @@ class DriveController(Writer):
     # extends the Writer parent class
     # handles differential drive controll for the robot
     ###
-    def __init__(self):
+    def __init__(self, hostname):
         Writer.__init__(
             self, ["mob.driveControlWheelSpeeds.l", "mob.driveControlWheelSpeeds.r"]
         )
-
-    def run(self, hostname):
+        # runs a subscriber for setting the wheel velocity.
         rospy.Subscriber("/" + hostname + "/cmd_vel", Twist, self.callback)
 
     def callback(self, cmd_msg):

hv_bridge/libs/Controllers/gripper.py

@@ -14,10 +14,12 @@ class GripperController(Writer):
     # Controller for the robot gripper
     # Extends the Setter class
     ###
-    def __init__(self):
+    def __init__(self, hostname):
         # Create writer for this probe
         # manip.gripperRequestedTargetPositionInPercent
         Writer.__init__(self, ["manip.gripperTargetPositionInPercent"])
+        # intializes the mp connection and runs a subscriber for setting the gripper position.
+        rospy.Subscriber("/" + hostname + "/set_gripper_pos", Float32, self.callback)
 
     def callback(self, cmd_msg):
         # callback function - extends the setter.callback functionality
@@ -28,7 +30,3 @@ class GripperController(Writer):
         elif pos < 0:
             pos = 0
         Writer.callback(self, pos)
-
-    def run(self, rospy, hostname):
-        # 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/__init__.py

@@ -11,4 +11,4 @@ from writer import *
 from config import *
 from utils import *
 
-from listener import * 
+from listener import *

hv_bridge/libs/Resources/client.py

@@ -172,6 +172,24 @@ class Client:
         except Exception as e:
             raise e
 
+    def lookup_probe_type(self, probe_id):
+        # get the probe type
+        try:
+            if type(probe_id) != int:
+                raise ValueError("probe id must be an int")
+            return self.probe_defs[probe_id][1]
+        except Exception as e:
+            raise e
+
+    def lookup_probe(self, t):
+        # get the probe info
+        try:
+            if type(t) != int:
+                raise ValueError("probe id must be an int")
+            return (self.probe_defs[t][0], self.probe_defs[t][1])
+        except Exception as e:
+            raise e
+
     def enable_probes(self, probes):
         # enables one or more probes.
         # allow for singleton argument

hv_bridge/libs/Resources/config.py

@@ -22,20 +22,20 @@ MP_TLV_MISSED_DEBUG_MESSAGES = 15
 MP_TLV_WRITE_PROBES = 16
 
 type_table = {
-            1: ("uint8", "<B"),  # 1-byte unsigned integer
-            2: ("int8", "<b"),  # 1-byte signed integer
-            3: ("uint16", "<H"),  # 2-byte unsigned integer
-            4: ("int16", "<h"),  # 2-byte signed integer
-            5: ("uint32", "<I"),  # 4-byte unsigned integer
-            6: ("int32", "<i"),  # 4-byte signed integer
-            7: ("uint64", "<Q"),  # 8-byte unsigned integer
-            8: ("int64", "<q"),  # 8-byte signed integer
-            9: ("float", "<f"),  # 4-byte float (IEEE 754)
-            10: ("double", "<d"),  # 8-byte float (IEEE 754)
-            11: ("tlv", ""),  # Variable-length TLV data
-            12: ("bool", "<B"),  # 1-byte boolean integer (1 or 0)
-            13: ("string", ""),  # variable-size null-terminated char string
-        }
+    1: ("uint8", "<B"),  # 1-byte unsigned integer
+    2: ("int8", "<b"),  # 1-byte signed integer
+    3: ("uint16", "<H"),  # 2-byte unsigned integer
+    4: ("int16", "<h"),  # 2-byte signed integer
+    5: ("uint32", "<I"),  # 4-byte unsigned integer
+    6: ("int32", "<i"),  # 4-byte signed integer
+    7: ("uint64", "<Q"),  # 8-byte unsigned integer
+    8: ("int64", "<q"),  # 8-byte signed integer
+    9: ("float", "<f"),  # 4-byte float (IEEE 754)
+    10: ("double", "<d"),  # 8-byte float (IEEE 754)
+    11: ("tlv", ""),  # Variable-length TLV data
+    12: ("bool", "<B"),  # 1-byte boolean integer (1 or 0)
+    13: ("string", ""),  # variable-size null-terminated char string
+}
 
 
 # Declare function types
@@ -63,18 +63,18 @@ AVOID_SRV = HV_Services.AVOID_SRV
 
 # Probe names
 
-START = "ui.fakeStartButtonNoSpacing" #1125
-LIDAR_STANDBY = "vis.sick.useStandByMode" #2877
-ARM_ENABLED = "manip.armEnableRequest" #1082
-GRIPPER_ENABLED = "manip.gripperEnableRequest" #1083
-DRIVE_ENABLED = "drive.enableRequest" #580
-DRIVE_COMMAND = "mob.driveCommand.enable" #1220
-ROB_SENSOR = "vis.robotSensor.enabled" #3100
-LINE_SENSOR = "vis.lineSensor.enabled" #3171
+START = "ui.fakeStartButtonNoSpacing"  # 1125
+LIDAR_STANDBY = "vis.sick.useStandByMode"  # 2877
+ARM_ENABLED = "manip.armEnableRequest"  # 1082
+GRIPPER_ENABLED = "manip.gripperEnableRequest"  # 1083
+DRIVE_ENABLED = "drive.enableRequest"  # 580
+DRIVE_COMMAND = "mob.driveCommand.enable"  # 1220
+ROB_SENSOR = "vis.robotSensor.enabled"  # 3100
+LINE_SENSOR = "vis.lineSensor.enabled"  # 3171
 ESTOP = "ui.fakeEstop"
 FLAG_PULL = "estop.fakeFlagPulled"
 
-BEEP = "ui.enableBeeping" #1120
+BEEP = "ui.enableBeeping"  # 1120
 
 HAS_POT = "manip.botGotPot"
 

hv_bridge/libs/Resources/listener.py

@@ -10,6 +10,7 @@ import sys
 import subprocess
 import rospy
 
+
 class listen(threading.Thread):
     # listener class for getting data from robot
     def __init__(self, q, client):

hv_bridge/libs/Resources/utils.py

@@ -30,7 +30,8 @@ def twist_to_vel(twist):
 def resource_name(name):
     # formats a hostname to be legaly used in a ros pubisher/subscriber name
     return ((name.replace("-", "")).replace(".", "_")).lower()
-    
+
+
 ###
 # functions that handle the formatting, reading and witing of tlv messages
 # tlv is the structure of message that mp uses to communicate

hv_bridge/libs/Resources/writer.py

@@ -9,7 +9,13 @@ class Writer:
 
     def get_msgs(self):
         # collect the messages to send
-        return self.msgs
+        m = self.msgs
+        self.msgs = []
+        return m
+
+    def clear_msgs(self):
+        # clear msg array
+        self.msgs = []
 
     def callback(self, msg):
         # subscriber callback function
@@ -21,7 +27,7 @@ class Writer:
             # each message coresponds to a probe (1 to 1)
             if len(msg) != len(self.probes):
                 raise Exception("message size missmatch")
-            self.msgs = zip(self.probes)
+            self.msgs = zip(self.probes, msg)
         else:
             # publish the same message to each of the probes
             self.msgs = [(c, msg) for c in self.probes]

hv_bridge/scripts/hv_client.py

@@ -36,13 +36,13 @@ class HVClient(Client):
         name = resource_name(hostname)
         if DRIVE_CMD in services:
             # enable drive controller
-            self.cmdr.append(DriveController())
+            self.cmdr.append(DriveController(name))
         if GRIPPER_CMD in services:
             # enable gripper controller
-            self.cmdr.append(GripperController())
+            self.cmdr.append(GripperController(name))
         if ARM_CMD in services:
             # enable arm controller
-            self.cmdr.append(ArmController())
+            self.cmdr.append(ArmController(name))
 
         if ODOM_LSTN in services:
             # enable default odometry listener
@@ -112,10 +112,10 @@ class HVClient(Client):
             # 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)])
+            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)])
+            Client.write_probes(self, [(ESTOP, 0), (FLAG_PULL, 0)])
             print str(self.hostname) + " has stopped all running processes"
         except Exception as e:
             raise e
@@ -142,25 +142,29 @@ class HVClient(Client):
             )
             msgs = get_msg_arr([], map((lambda a: a.get_msgs()), self.cmdr))
             v = ""
-            if len(msgs) == 1:
-                msgs = ((msgs),)
+            if len(msgs) == 0:
+                return
+            print msgs
+            print len(msgs)
             for (probe, value) in msgs:
-                probe_id = Client.lookup_probe_id(self,probe)
-                probe_type = self.probe_defs[probe_id][1]
+                probe_id = Client.lookup_probe_id(self, probe)
+                probe_type = Client.lookup_probe_type(self, probe_id)
 
-                v += make_tlv(probe_id, Client.encode_probe_value(self,probe_type, value))
-            Client.send_message(self,make_tlv(MP_TLV_WRITE_PROBES, v))
+                v += make_tlv(
+                    probe_id, Client.encode_probe_value(self, probe_type, value)
+                )
+            Client.send_message(self, make_tlv(MP_TLV_WRITE_PROBES, v))
         except Exception as e:
-            raise e
+            rospy.logwarn(e)
+            # raise e
 
     def read(self, t, v):
         # pass data to the correct child class
         try:
-            name = self.probe_defs[t][0]
-            type = self.probe_defs[t][1]
-            val = Client.decode_probe_data(self, type, v)
+            (pname, ptype) = Client.lookup_probe(self, t)
+            val = Client.decode_probe_data(self, ptype, v)
 
-            [serv.add(name, val) for serv in self.lstnr]
+            [serv.add(pname, val) for serv in self.lstnr]
 
         except Exception as e:
             raise e

hv_bridge/scripts/main_node.py

@@ -87,7 +87,7 @@ if __name__ == "__main__":
             "sys",
             type=str,
             nargs="*",
-            help="System included variables from launch file (not set bu user)"
+            help="System included variables from launch file (not set bu user)",
         )
 
         args = parser.parse_args()
@@ -177,7 +177,7 @@ if __name__ == "__main__":
         # default to 100. (mp runs at 200)
         hz = 100
         if hv_bot.hz:
-            hz = hv_bot.hz/2
+            hz = hv_bot.hz / 2
 
         # Start the ROS main loop
         rate = rospy.Rate(hz)