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
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)
#!/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)
#!/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)
......@@ -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 *
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
......@@ -15,7 +15,10 @@ class Reader:
def add(name, val):
# add data piece
try:
self.queue[name] = val
if name in self.probes:
self.queue[name] = val
else:
pass
except Exception as e:
raise e
......
......@@ -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]
......@@ -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)
......@@ -68,11 +73,11 @@ class HVClient(Client):
if AVOID_SRV in services:
# enable odometry with gryro correction listener
self.srvs.append(AvoidService())
Client(self)
Super.__init__()
except Exception as e:
raise e
def start(self, silent=True):
# start up ROS bridge mode for robot
try:
......@@ -112,17 +117,27 @@ 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))
v = ""
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),)
for (probe, value) in 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
......@@ -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
......
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