update controllers to use writer parent class

hv_bridge/libs/Controllers/arm.py

 #!/usr/bin/env python
+import rospy
 from std_msgs.msg import Float32
 import sys, os
 
-SCRIPTS_PATH = os.path.abspath(os.path.join(sys.path[0], "../../"))
-sys.path.append(SCRIPTS_PATH)
+RESOURCE_PATH = os.path.abspath(os.path.join(sys.path[0], "../../"))
+sys.path.append(RESOURCE_PATH)
 
-from libs.Resources import Setter
+from libs.Resources import Writer
 
 
-class ArmController(Setter):
+class ArmController(Writer):
     ###
     # Controller for the robot arm
     # Extends the Setter class
     ###
+    def __init__(self):
+        Writer(["manip.armTargetPositionInPercent"])
+    
+    def run(self, hostname):
+        #runs a subscriber for setting the arm position.
+        rospy.Subscriber("/" + hostname + "/set_arm_pos", Float32, self.callback)
+    
     def callback(self, cmd_msg):
         # callback function - extends the setter.callback functionality
         # formats data to be passed to mindprobe
@@ -21,9 +29,4 @@ class ArmController(Setter):
             pos = 100
         elif pos < 0:
             pos = 0
-        Setter.callback(self, pos)
-
-    def run(self, rospy, hostname):
-        # intializes the mp connection and runs a subscriber for setting the arm position.
-        Setter.init(self, 1077)
-        rospy.Subscriber("/" + hostname + "/set_arm_pos", Float32, self.callback)
+        Writer.callback(self, pos)

hv_bridge/libs/Controllers/drive.py

 #!/usr/bin/env python
+import rospy
 from geometry_msgs.msg import Twist
 from std_msgs.msg import Float64
 
 import sys, os
 
-SCRIPTS_PATH = os.path.abspath(os.path.join(sys.path[0], "../../"))
-sys.path.append(SCRIPTS_PATH)
+RESOURCE_PATH = os.path.abspath(os.path.join(sys.path[0], "../../"))
+sys.path.append(RESOURCE_PATH)
 
-from libs.Resources import Setter
+from libs.Resources import Writer
 from libs.Resources.utils import *
 
 
-class DriveController(Setter):
+class DriveController(Writer):
     ###
     # Controller for the robot drive
-    # extends the setter parent class
-    # handles individual wheel commands as well as a differential drive
+    # extends the Writer parent class
+    # handles differential drive controll for the robot
     ###
+    def __init__(self):
+        Writer(["mob.driveControlWheelSpeeds.l","mob.driveControlWheelSpeeds.r"])
+    
+    def run(self, hostname):
+        rospy.Subscriber("/" + hostname + "/cmd_vel", Twist, self.callback)
+    
     def callback(self, cmd_msg):
         # differential drive callback.
         vel = twist_to_vel(cmd_msg)
-        Setter.callback(self, vel)
-
-    def set_left(self, cmd_msg):
-        # callback to set the left wheel
-        vel = cmd_msg.data
-        Setter.callback(self, [vel, 0])
-
-    def set_right(self, cmd_msg):
-        # callback to set the right wheel
-        vel = cmd_msg.data
-        Setter.callback(self, [0, vel])
-
-    def run(self, rospy, hostname):
-        # initialize the connection and create subscribers
-        Setter.init(self, (1206, 1207))
-
-        rospy.Subscriber("/" + hostname + "/cmd_vel", Twist, self.callback)
-        rospy.Subscriber("/" + hostname + "/cmd_vel_left", Float64, self.set_left)
-        rospy.Subscriber("/" + hostname + "/cmd_vel_right", Float64, self.set_right)
+        Writer.callback(self, vel)

hv_bridge/libs/Controllers/gripper.py

 #!/usr/bin/env python
+import rospy
 from std_msgs.msg import Float32
 import sys, os
 
-SCRIPTS_PATH = os.path.abspath(os.path.join(sys.path[0], "../../"))
-sys.path.append(SCRIPTS_PATH)
+RESOURCE_PATH = os.path.abspath(os.path.join(sys.path[0], "../../"))
+sys.path.append(RESOURCE_PATH)
 
-from libs.Resources import Setter
+from libs.Resources import Writer
 
 
-class GripperController(Setter):
+class GripperController(Writer):
     ###
     # Controller for the robot gripper
     # Extends the Setter class
     ###
+    def __init__(self):
+        Writer(["manip.gripperTargetPositionInPercent"])
     def callback(self, cmd_msg):
         # callback function - extends the setter.callback functionality
         # formats data to be passed to mindprobe
@@ -21,10 +24,8 @@ class GripperController(Setter):
             pos = 100
         elif pos < 0:
             pos = 0
-        Setter.callback(self, pos)
+        Writer.callback(self, pos)
 
     def run(self, rospy, hostname):
         # intializes the mp connection and runs a subscriber for setting the gripper position.
-        Setter.init(self, 1080)
-
         rospy.Subscriber("/" + hostname + "/set_gripper_pos", Float32, self.callback)

hv_bridge/libs/Resources/__init__.py

@@ -4,13 +4,9 @@
 # include all the utility scripts
 ###
 
-from client import Client
-from reader import Reader
-from writer import Writer
-from config import Serv
-
+from client import *
+from reader import *
+from writer import *
 
+from config import *
 from utils import *
-from getter import *
-from setter import *
-from mp import *

hv_bridge/libs/Resources/config.py

-import enum.Enum
+# from enum import Enum
 
-class Serv(Enum):
-	DRIVE_CMD = 1
-	GRIPPER_CMD = 2
-	ARM_CMD = 3
-	ODOM_CMD = 4
-    ODOM_LSTN = 5
-	IR_LSTN = 6
-	LIDAR_LSTN = 7
-	GRIPPER_LSTN = 8
-	NEAREST_OBS = 9
-	ODOM_GYRO_LSTN = 10
-    FOLLOW_SRV = 11
-	PICK_SRV = 12
-	AVOID_SRV = 13
+# class Serv(Enum):
+# TODO: switch this to enum
+DRIVE_CMD = 1
+GRIPPER_CMD = 2
+ARM_CMD = 3
+ODOM_CMD = 4
+ODOM_LSTN = 5
+IR_LSTN = 6
+LIDAR_LSTN = 7
+GRIPPER_LSTN = 8
+NEAREST_OBS = 9
+ODOM_GYRO_LSTN = 10
+FOLLOW_SRV = 11
+PICK_SRV = 12
+AVOID_SRV = 13

hv_bridge/libs/Resources/reader.py

@@ -15,7 +15,10 @@ class Reader:
     def add(name, val):
         # add data piece
         try:
+            if name in self.probes:
                 self.queue[name] = val
+            else:
+                pass
         except Exception as e:
             raise e
 

hv_bridge/libs/Resources/writer.py

@@ -3,6 +3,7 @@ class Writer:
     def __init__(self, p):
         # constructor for settings class
         self.probes = p
+        self.msgs = []
 
     def get_msgs():
         # collect the messages to send
@@ -10,22 +11,15 @@ class Writer:
 
     def callback(self, msg):
         # subscriber callback function
-        if type(self.code) != tuple:
+        if len(self.probes) == 1:
             # if the message is a single item value pair
-            self.msgs = [(self.code, msg)]
+            self.msgs = [(self.probes, msg)]
         elif len(msg) > 1:
             # handle an array of messages
             # each message coresponds to a probe (1 to 1)
-            # if(msg.length != self.code.length) throw "message size missmatch"
-            command = []
-            i = 0
-            for c in self.code:
-                command.append((c, msg[i]))
-                i = i + 1
-            self.msgs = command
+            if(len(msg) != len(self.probes)):
+                raise Exception("message size missmatch")
+            self.msgs = zip(self.probes)
         else:
             # publish the same message to each of the probes
-            command = []
-            for c in self.code:
-                command.append((c, msg))
-            self.msgs = command
+            self.msgs = [(c,msg) for c in self.probes]

hv_bridge/scripts/hv_client.py

@@ -9,6 +9,12 @@ import os
 import sys
 import subprocess
 
+# add local python scripts to the path so that they can be iported
+sys.path.append(os.path.abspath(os.path.join(sys.path[0], "../")))
+sys.path.append(os.path.abspath(os.path.join(sys.path[0], "../srv")))
+
+from libs import *
+
 ### --------- ###
 #
 ### Harvest Vehicle Client Connection ###
@@ -19,7 +25,6 @@ import subprocess
 
 
 class HVClient(Client):
-
     def __init__(self, services):
         try:
             # enable services, selection default is all (0)
@@ -69,7 +74,7 @@ class HVClient(Client):
                 # enable odometry with gryro correction listener
                 self.srvs.append(AvoidService())
 
-            Client(self)
+            Super.__init__()
         except Exception as e:
             raise e
 
@@ -112,16 +117,26 @@ class HVClient(Client):
     def enable_services():
         # take all probes that need to be listened for and enable them
         try:
-            get_probe_arr = lambda a,b : get_probe_arr(a.union(b[0]),b[1:]) if len(b) > 1 else list(a.union(b[0]))
-            probes = get_probe_arr(set([]),map((lambda a : set(a.get_probes())),self.lstnr))
-
-            Client.enable_probes(self,probes)
+            get_probe_arr = (
+                lambda a, b: get_probe_arr(a.union(b[0]), b[1:])
+                if len(b) > 1
+                else list(a.union(b[0]))
+            )
+            probes = get_probe_arr(
+                set([]), map((lambda a: set(a.get_probes())), self.lstnr)
+            )
+
+            Client.enable_probes(self, probes)
+        except Exception as e:
+            raise e
 
     def write(self):
         # gather all messages from controll class instances and write to the robot
         try:
-            get_msg_arr = lambda a,b : get_msg_arr(a+b[0],b[1:]) if len(b) > 1 else a + b[0])
-            msgs = get_probe_arr([],map((lambda a : a.get_msgs()),self.cmdr))
+            get_msg_arr = (
+                lambda a, b: get_msg_arr(a + b[0], b[1:]) if len(b) > 1 else a + b[0]
+            )
+            msgs = get_probe_arr([], map((lambda a: a.get_msgs()), self.cmdr))
             v = ""
             if type(msgs) != tuple:
                 msgs = ((msgs),)
@@ -134,14 +149,14 @@ class HVClient(Client):
         except Exception as e:
             raise e
 
-    def read(t,v):
+    def read(t, v):
         # pass data to the correct child class
         try:
             name = Client.probe_defs[t][0]
             type = Client.probe_defs[t][1]
             val = Client.decode_probe_data(type, v)
 
-            (serv.add(name, val) if serv.needs(name) else pass) for serv in self.lstnr
+            [serv.add(name, val) for serv in self.lstnr]
 
         except Exception as e:
             raise e

hv_bridge/scripts/main_node.py

@@ -8,7 +8,9 @@ import sys, os, time
 sys.path.append(os.path.abspath(os.path.join(sys.path[0], "../")))
 sys.path.append(os.path.abspath(os.path.join(sys.path[0], "../srv")))
 
-from libs import *
+from hv_client import *
+
+# from libs import *
 
 # Global variables
 host_name = "hai-1095.local"  # this is the hostname the robot being controlled.
@@ -153,6 +155,7 @@ if __name__ == "__main__":
 
         print("connecting to {}").format(host_name)
 
+        print(services)
         hv_bot = HVClient(services)
 
         # start the connection to the robot