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

update controllers to use writer parent class

parent b302efa2
#!/usr/bin/env python #!/usr/bin/env python
import rospy
from std_msgs.msg import Float32 from std_msgs.msg import Float32
import sys, os import sys, os
SCRIPTS_PATH = os.path.abspath(os.path.join(sys.path[0], "../../")) RESOURCE_PATH = os.path.abspath(os.path.join(sys.path[0], "../../"))
sys.path.append(SCRIPTS_PATH) 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 # Controller for the robot arm
# Extends the Setter class # 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): def callback(self, cmd_msg):
# callback function - extends the setter.callback functionality # callback function - extends the setter.callback functionality
# formats data to be passed to mindprobe # formats data to be passed to mindprobe
...@@ -21,9 +29,4 @@ class ArmController(Setter): ...@@ -21,9 +29,4 @@ class ArmController(Setter):
pos = 100 pos = 100
elif pos < 0: elif pos < 0:
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 arm position.
Setter.init(self, 1077)
rospy.Subscriber("/" + hostname + "/set_arm_pos", Float32, self.callback)
#!/usr/bin/env python #!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist from geometry_msgs.msg import Twist
from std_msgs.msg import Float64 from std_msgs.msg import Float64
import sys, os import sys, os
SCRIPTS_PATH = os.path.abspath(os.path.join(sys.path[0], "../../")) RESOURCE_PATH = os.path.abspath(os.path.join(sys.path[0], "../../"))
sys.path.append(SCRIPTS_PATH) sys.path.append(RESOURCE_PATH)
from libs.Resources import Setter from libs.Resources import Writer
from libs.Resources.utils import * from libs.Resources.utils import *
class DriveController(Setter): class DriveController(Writer):
### ###
# Controller for the robot drive # Controller for the robot drive
# extends the setter parent class # extends the Writer parent class
# handles individual wheel commands as well as a differential drive # 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): def callback(self, cmd_msg):
# differential drive callback. # differential drive callback.
vel = twist_to_vel(cmd_msg) vel = twist_to_vel(cmd_msg)
Setter.callback(self, vel) Writer.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)
#!/usr/bin/env python #!/usr/bin/env python
import rospy
from std_msgs.msg import Float32 from std_msgs.msg import Float32
import sys, os import sys, os
SCRIPTS_PATH = os.path.abspath(os.path.join(sys.path[0], "../../")) RESOURCE_PATH = os.path.abspath(os.path.join(sys.path[0], "../../"))
sys.path.append(SCRIPTS_PATH) 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 # Controller for the robot gripper
# Extends the Setter class # Extends the Setter class
### ###
def __init__(self):
Writer(["manip.gripperTargetPositionInPercent"])
def callback(self, cmd_msg): def callback(self, cmd_msg):
# callback function - extends the setter.callback functionality # callback function - extends the setter.callback functionality
# formats data to be passed to mindprobe # formats data to be passed to mindprobe
...@@ -21,10 +24,8 @@ class GripperController(Setter): ...@@ -21,10 +24,8 @@ class GripperController(Setter):
pos = 100 pos = 100
elif pos < 0: elif pos < 0:
pos = 0 pos = 0
Setter.callback(self, pos) Writer.callback(self, pos)
def run(self, rospy, hostname): def run(self, rospy, hostname):
# intializes the mp connection and runs a subscriber for setting the gripper position. # 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) rospy.Subscriber("/" + hostname + "/set_gripper_pos", Float32, self.callback)
...@@ -4,13 +4,9 @@ ...@@ -4,13 +4,9 @@
# include all the utility scripts # include all the utility scripts
### ###
from client import Client from client import *
from reader import Reader from reader import *
from writer import Writer from writer import *
from config import Serv
from config import *
from utils import * from utils import *
from getter import *
from setter import *
from mp import *
import enum.Enum # from enum import Enum
class Serv(Enum): # class Serv(Enum):
DRIVE_CMD = 1 # TODO: switch this to enum
GRIPPER_CMD = 2 DRIVE_CMD = 1
ARM_CMD = 3 GRIPPER_CMD = 2
ODOM_CMD = 4 ARM_CMD = 3
ODOM_LSTN = 5 ODOM_CMD = 4
IR_LSTN = 6 ODOM_LSTN = 5
LIDAR_LSTN = 7 IR_LSTN = 6
GRIPPER_LSTN = 8 LIDAR_LSTN = 7
NEAREST_OBS = 9 GRIPPER_LSTN = 8
ODOM_GYRO_LSTN = 10 NEAREST_OBS = 9
FOLLOW_SRV = 11 ODOM_GYRO_LSTN = 10
PICK_SRV = 12 FOLLOW_SRV = 11
AVOID_SRV = 13 PICK_SRV = 12
AVOID_SRV = 13
...@@ -15,7 +15,10 @@ class Reader: ...@@ -15,7 +15,10 @@ class Reader:
def add(name, val): def add(name, val):
# add data piece # add data piece
try: try:
if name in self.probes:
self.queue[name] = val self.queue[name] = val
else:
pass
except Exception as e: except Exception as e:
raise e raise e
......
...@@ -3,6 +3,7 @@ class Writer: ...@@ -3,6 +3,7 @@ class Writer:
def __init__(self, p): def __init__(self, p):
# constructor for settings class # constructor for settings class
self.probes = p self.probes = p
self.msgs = []
def get_msgs(): def get_msgs():
# collect the messages to send # collect the messages to send
...@@ -10,22 +11,15 @@ class Writer: ...@@ -10,22 +11,15 @@ class Writer:
def callback(self, msg): def callback(self, msg):
# subscriber callback function # subscriber callback function
if type(self.code) != tuple: if len(self.probes) == 1:
# if the message is a single item value pair # if the message is a single item value pair
self.msgs = [(self.code, msg)] self.msgs = [(self.probes, msg)]
elif len(msg) > 1: elif len(msg) > 1:
# handle an array of messages # handle an array of messages
# each message coresponds to a probe (1 to 1) # each message coresponds to a probe (1 to 1)
# if(msg.length != self.code.length) throw "message size missmatch" if(len(msg) != len(self.probes)):
command = [] raise Exception("message size missmatch")
i = 0 self.msgs = zip(self.probes)
for c in self.code:
command.append((c, msg[i]))
i = i + 1
self.msgs = command
else: else:
# publish the same message to each of the probes # publish the same message to each of the probes
command = [] self.msgs = [(c,msg) for c in self.probes]
for c in self.code:
command.append((c, msg))
self.msgs = command
...@@ -9,6 +9,12 @@ import os ...@@ -9,6 +9,12 @@ import os
import sys import sys
import subprocess 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 ### ### Harvest Vehicle Client Connection ###
...@@ -19,7 +25,6 @@ import subprocess ...@@ -19,7 +25,6 @@ import subprocess
class HVClient(Client): class HVClient(Client):
def __init__(self, services): def __init__(self, services):
try: try:
# enable services, selection default is all (0) # enable services, selection default is all (0)
...@@ -69,7 +74,7 @@ class HVClient(Client): ...@@ -69,7 +74,7 @@ class HVClient(Client):
# enable odometry with gryro correction listener # enable odometry with gryro correction listener
self.srvs.append(AvoidService()) self.srvs.append(AvoidService())
Client(self) Super.__init__()
except Exception as e: except Exception as e:
raise e raise e
...@@ -112,16 +117,26 @@ class HVClient(Client): ...@@ -112,16 +117,26 @@ class HVClient(Client):
def enable_services(): def enable_services():
# take all probes that need to be listened for and enable them # take all probes that need to be listened for and enable them
try: 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])) get_probe_arr = (
probes = get_probe_arr(set([]),map((lambda a : set(a.get_probes())),self.lstnr)) lambda a, b: get_probe_arr(a.union(b[0]), b[1:])
if len(b) > 1
Client.enable_probes(self,probes) 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): def write(self):
# gather all messages from controll class instances and write to the robot # gather all messages from controll class instances and write to the robot
try: try:
get_msg_arr = lambda a,b : get_msg_arr(a+b[0],b[1:]) if len(b) > 1 else a + b[0]) get_msg_arr = (
msgs = get_probe_arr([],map((lambda a : a.get_msgs()),self.cmdr)) 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 = "" v = ""
if type(msgs) != tuple: if type(msgs) != tuple:
msgs = ((msgs),) msgs = ((msgs),)
...@@ -134,14 +149,14 @@ class HVClient(Client): ...@@ -134,14 +149,14 @@ class HVClient(Client):
except Exception as e: except Exception as e:
raise e raise e
def read(t,v): def read(t, v):
# pass data to the correct child class # pass data to the correct child class
try: try:
name = Client.probe_defs[t][0] name = Client.probe_defs[t][0]
type = Client.probe_defs[t][1] type = Client.probe_defs[t][1]
val = Client.decode_probe_data(type, v) 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: except Exception as e:
raise e raise e
...@@ -8,7 +8,9 @@ import sys, os, time ...@@ -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], "../")))
sys.path.append(os.path.abspath(os.path.join(sys.path[0], "../srv"))) 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 # Global variables
host_name = "hai-1095.local" # this is the hostname the robot being controlled. host_name = "hai-1095.local" # this is the hostname the robot being controlled.
...@@ -153,6 +155,7 @@ if __name__ == "__main__": ...@@ -153,6 +155,7 @@ if __name__ == "__main__":
print("connecting to {}").format(host_name) print("connecting to {}").format(host_name)
print(services)
hv_bot = HVClient(services) hv_bot = HVClient(services)
# start the connection to the robot # start the connection to the robot
......
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