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

cleanup errors

parent 4902325c
......@@ -17,12 +17,12 @@ class ArmController(Writer):
def __init__(self):
# Create writer for this probe
# manip.armRequestedTargetPositionInPercent
Writer(["manip.armTargetPositionInPercent"])
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)
def callback(self, cmd_msg):
# callback function - extends the setter.callback functionality
# formats data to be passed to mindprobe
......
......@@ -19,11 +19,13 @@ class DriveController(Writer):
# handles differential drive controll for the robot
###
def __init__(self):
Writer(["mob.driveControlWheelSpeeds.l","mob.driveControlWheelSpeeds.r"])
Writer.__init__(
self, ["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)
......
......@@ -17,7 +17,8 @@ class GripperController(Writer):
def __init__(self):
# Create writer for this probe
# manip.gripperRequestedTargetPositionInPercent
Writer(["manip.gripperTargetPositionInPercent"])
Writer.__init__(self, ["manip.gripperTargetPositionInPercent"])
def callback(self, cmd_msg):
# callback function - extends the setter.callback functionality
# formats data to be passed to mindprobe
......
......@@ -30,16 +30,13 @@ import subprocess
class Client:
def __init__(self, host):
try:
self.capture_buffer = ()
self.probe_names = {}
self.probe_defs = {}
self.probe_list = []
self.listener_pipe = None
self.capture_buffer = ()
self.probe_names = {}
self.probe_defs = {}
self.probe_list = []
self.listener_pipe = None
self.connect(host)
except Exception as e:
raise e
self.connect(host)
def connect(self, host, port=4400):
# Makes a connection to the robot.
......
# from enum import Enum
# 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
from enum import Enum
HV_Services = Enum(
"HV_Services",
"DRIVE_CMD GRIPPER_CMD ARM_CMD ODOM_CMD ODOM_LSTN IR_LSTN LIDAR_LSTN GRIPPER_LSTN NEAREST_OBS ODOM_GYRO_LSTN FOLLOW_SRV PICK_SRV AVOID_SRV",
)
DRIVE_CMD = HV_Services.DRIVE_CMD
GRIPPER_CMD = HV_Services.GRIPPER_CMD
ARM_CMD = HV_Services.ARM_CMD
ODOM_CMD = HV_Services.ODOM_CMD
ODOM_LSTN = HV_Services.ODOM_LSTN
IR_LSTN = HV_Services.IR_LSTN
LIDAR_LSTN = HV_Services.LIDAR_LSTN
GRIPPER_LSTN = HV_Services.GRIPPER_LSTN
NEAREST_OBS = HV_Services.NEAREST_OBS
ODOM_GYRO_LSTN = HV_Services.ODOM_GYRO_LSTN
FOLLOW_SRV = HV_Services.FOLLOW_SRV
PICK_SRV = HV_Services.PICK_SRV
AVOID_SRV = HV_Services.AVOID_SRV
HAS_POT = "manip.botGotPot"
......@@ -39,7 +42,7 @@ ODOM_H = "loc.rawOdometry.h"
ODOM_V = "loc.rawOdometry.v"
ODOM_W = "loc.rawOdometry.w"
ODOM_GRYO_X = "loc.rawOdometryWGyro.x"
ODOM_GYRO_X = "loc.rawOdometryWGyro.x"
ODOM_GYRO_Y = "loc.rawOdometryWGyro.y"
ODOM_GYRO_H = "loc.rawOdometryWGyro.h"
ODOM_GYRO_V = "loc.rawOdometryWGyro.v"
......
......@@ -11,7 +11,7 @@ class Reader:
## Publish data
self.pub.publish(data)
def add(name, val):
def add(self, name, val):
# add data piece
try:
if name in self.probes:
......@@ -19,10 +19,10 @@ class Reader:
except Exception as e:
raise e
def needs(name):
def needs(self, name):
# check if name is used by this class
return True if name in self.probes else False
def get_probes():
def get_probes(self):
# return an array of all probes
return self.probes if len(self.probes) > 1 else self.probes[0]
class Writer:
# reader class for getting data from the robot and passing to ROS
msgs = []
probes = []
def __init__(self, p):
# constructor for settings class
self.probes = p
self.msgs = []
def get_msgs():
def get_msgs(self):
# collect the messages to send
return self.msgs
......@@ -17,9 +19,9 @@ class Writer:
elif len(msg) > 1:
# handle an array of messages
# each message coresponds to a probe (1 to 1)
if(len(msg) != len(self.probes)):
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
self.msgs = [(c,msg) for c in self.probes]
self.msgs = [(c, msg) for c in self.probes]
......@@ -4,4 +4,12 @@
# include all the sensor scripts for import to the services node
###
from hub import SensorHub
from gripper import GripperListener
from ir import (
IrListenerRightFront,
IrListenerLeftFront,
IrListenerRightBack,
IrListenerLeftBack,
)
from lidar import LidarListener, LidarNObsListener
from odometry import OdometryListener, OdometryGyroListener
......@@ -11,7 +11,7 @@ from libs.Resources.utils import *
from libs.Resources.config import *
class Gripper(Reader):
class GripperListener(Reader):
###
# Gripper information class extending the getter, for handling the publishing of data
###
......@@ -20,9 +20,9 @@ class Gripper(Reader):
def __init__(self, h):
self.hostname = h
Reader([HAS_POT])
Reader.__init__(self, [HAS_POT])
self.pub = rospy.Publisher(
"/" + self.hostname + "/gripper/" + resource_name(self.get_probes()),
"/" + self.hostname + "/gripper/" + resource_name(Reader.get_probes(self)),
Bool,
queue_size=1,
)
......
......@@ -18,7 +18,7 @@ class IrListener(Reader):
def __init__(self, h, p):
self.hostname = h
Reader(p)
Reader.__init__(self, p)
self.pub = rospy.Publisher(
"/"
+ self.hostname
......@@ -32,19 +32,26 @@ class IrListener(Reader):
## handles the publishing of data, should be run at an acceptable update interval
# get data from queue
data_list = [self.q[label] for label in self.probes]
data = sum(data_list)/len(data_list)
data = sum(data_list) / len(data_list)
# publish data
Reader.pub(self, data)
class IrListenerRightFront(IrListener):
def __init__(self, h):
IrListener(h, [IR_RIGHT_FRONT_A,IR_RIGHT_FRONT_B])
IrListener.__init__(self, h, [IR_RIGHT_FRONT_A, IR_RIGHT_FRONT_B])
class IrListenerLeftFront(IrListener):
def __init__(self, h):
IrListener(h, [IR_LEFT_FRONT_A,IR_LEFT_FRONT_B])
IrListener.__init__(self, h, [IR_LEFT_FRONT_A, IR_LEFT_FRONT_B])
class IrListenerRightBack(IrListener):
def __init__(self, h):
IrListener(h, [IR_RIGHT_BACK_A,IR_RIGHT_BACK_B])
IrListener.__init__(self, h, [IR_RIGHT_BACK_A, IR_RIGHT_BACK_B])
class IrListenerLeftBack(IrListener):
def __init__(self, h):
IrListener(h, [IR_LEFT_BACK_A,IR_LEFT_BACK_B])
IrListener.__init__(self, h, [IR_LEFT_BACK_A, IR_LEFT_BACK_B])
......@@ -21,16 +21,17 @@ from libs.Resources import Reader
from libs.Resources.utils import *
from libs.Resources.config import *
class LidarListener(Reader):
###
# Lidar sensor class extending the getter, for handling the publishing of data
###
def __init__(self, h):
self.hostname = h
Reader([LOCAL_SCAN])
self.pub = ros.Publisher(
"/" + hostname + "/lidar/" + resource_name(self.get_probes()),
Reader.__init__(self, [LOCAL_SCAN])
self.pub = rospy.Publisher(
"/" + self.hostname + "/lidar/" + resource_name(Reader.get_probes(self)),
PointCloud2,
queue_size=1,
)
......@@ -78,7 +79,7 @@ class LidarListener(Reader):
except Exception as e:
# well this is sad
err = traceback.format_exc()
print (err)
print(err)
class LidarNObsListener(Reader):
......@@ -87,12 +88,12 @@ class LidarNObsListener(Reader):
###
def __init__(self, h):
self.hostname = h
Reader([OBSTACLE_X,OBSTACLE_Y])
self.pub = ros.Publisher(
Reader.__init__(self, [OBSTACLE_X, OBSTACLE_Y])
self.pub = rospy.Publisher(
"/"
+ hostname
+ self.hostname
+ "/lidar/"
+ resource_name(os.path.commonprefix(self.get_probes())),
+ resource_name(os.path.commonprefix(Reader.get_probes(self))),
PointStamped,
queue_size=1,
)
......@@ -118,6 +119,4 @@ class LidarNObsListener(Reader):
except Exception as e:
# well this is sad
err = traceback.format_exc()
print (err)
print(err)
......@@ -25,27 +25,28 @@ class Odometer(Reader):
###
# Odometer class for reading the positional state data and for handling the publishing of data
###
def __init__(self, h, p):
self.hostname = h
Reader(self,p)
self.pub = ros.Publisher(
Reader.__init__(self, p)
self.pub = rospy.Publisher(
"/"
+ hostname
+ self.hostname
+ "/odom/"
+ resource_name(os.path.commonprefix(self.get_probes())),
+ resource_name(os.path.commonprefix(Reader.get_probes(self))),
Odometry,
queue_size=1,
)
def pub(self):
## handles the publishing of data, should be run at an acceptable update interval
# get data from queue
odom = Odometry()
odom.header.stamp = rospy.get_rostime()
odom.header.frame_id = self.hostname
[X,Y,H,V,W] = list(map(self.q.get,self.get_probes()))
[X, Y, H, V, W] = list(map(self.q.get, Reader.get_probes(self)))
heading = tf.transformations.quaternion_from_euler(0.0, 0.0, H)
odom.pose.pose = Pose(
Point(X, Y, 0.0),
......@@ -54,13 +55,17 @@ class Odometer(Reader):
odom.twist.twist = Twist(Vector3(V, 0, 0), Vector3(0, 0, W))
# publish data
Reader.pub(self, odom)
class OdometryListener(Odometer):
# Basic Odometer
def __init__(self,h):
Odometer(self,h,[ODOM_X,ODOM_Y,ODOM_H,ODOM_V,ODOM_W])
# Basic Odometer
def __init__(self, h):
Odometer.__init__(self, h, [ODOM_X, ODOM_Y, ODOM_H, ODOM_V, ODOM_W])
class OdometryGyroListener(Odometer):
# Odometer with Gyro corrections
def __init__(self,h):
Odometer(self,h,[ODOM_GYRO_X,ODOM_GYRO_Y,ODOM_GYRO_H,ODOM_GYRO_V,ODOM_GYRO_W])
def __init__(self, h):
Odometer.__init__(
self, h, [ODOM_GYRO_X, ODOM_GYRO_Y, ODOM_GYRO_H, ODOM_GYRO_V, ODOM_GYRO_W]
)
......@@ -25,61 +25,63 @@ from libs import *
class HVClient(Client):
cmdr = []
lstnr = []
srvs = []
using_lidar = False
# constructor for the hv bridge instance of the client connection
def __init__(self, hostname, services):
try:
# enable services, selection default is all (0)
self.cmdr = []
self.lstnr = []
self.using_lidar = False
if DRIVE_CMD in services:
# enable drive controller
self.cmdr.append(DriveController(hostname))
if GRIPPER_CMD in services:
# enable gripper controller
self.cmdr.append(GripperController(hostname))
if ARM_CMD in services:
# enable arm controller
self.cmdr.append(ArmController(hostname))
if ODOM_LSTN in services:
# enable default odometry listener
self.lstnr.append(OdometryListener(hostname))
if IR_LSTN in services:
# enable default ir listeners
self.lstnr.append(IrListenerRightFront(hostname))
self.lstnr.append(IrListenerLeftFront(hostname))
self.lstnr.append(IrListenerRightBack(hostname))
self.lstnr.append(IrListenerLeftBack(hostname))
if LIDAR_LSTN in services:
# enable default lidar listener
self.using_lidar = True
self.lstnr.append(LidarListener(hostname))
if GRIPPER_LSTN in services:
# enable default gripper has pot detection
self.lstnr.append(GripperListener(hostname))
if NEAREST_OBS in service:
# enable nearest obsticale location listener
self.using_lidar = True
self.lstnr.append(LidarNObsListener(hostname))
if ODOM_GYRO_LSTN in services:
# enable odometry with gryro correction listener
self.lstnr.append(OdometryGyroListener(hostname))
if FOLLOW_SRV in services:
# enable odometry with gryro correction listener
self.srvs.append(FollowService())
if PICK_SRV in services:
# enable odometry with gryro correction listener
self.svrs.append(PickTargetService())
if AVOID_SRV in services:
# enable odometry with gryro correction listener
self.srvs.append(AvoidService())
Client()
except Exception as e:
raise e
name = resource_name(hostname)
if DRIVE_CMD in services:
# enable drive controller
self.cmdr.append(DriveController())
if GRIPPER_CMD in services:
# enable gripper controller
self.cmdr.append(GripperController())
if ARM_CMD in services:
# enable arm controller
self.cmdr.append(ArmController())
if ODOM_LSTN in services:
# enable default odometry listener
self.lstnr.append(OdometryListener(name))
if IR_LSTN in services:
# enable default ir listeners
self.lstnr.append(IrListenerRightFront(name))
self.lstnr.append(IrListenerLeftFront(name))
self.lstnr.append(IrListenerRightBack(name))
self.lstnr.append(IrListenerLeftBack(name))
if LIDAR_LSTN in services:
# enable default lidar listener
self.using_lidar = True
self.lstnr.append(LidarListener(name))
if GRIPPER_LSTN in services:
# enable default gripper has pot detection
self.lstnr.append(GripperListener(name))
if NEAREST_OBS in services:
# enable nearest obsticale location listener
self.using_lidar = True
self.lstnr.append(LidarNObsListener(name))
if ODOM_GYRO_LSTN in services:
# enable odometry with gryro correction listener
self.lstnr.append(OdometryGyroListener(name))
if FOLLOW_SRV in services:
# enable odometry with gryro correction listener
# self.srvs.append(FollowService())
pass
if PICK_SRV in services:
# enable odometry with gryro correction listener
# self.svrs.append(PickTargetService())
pass
if AVOID_SRV in services:
# enable odometry with gryro correction listener
# self.srvs.append(AvoidService())
pass
Client.__init__(self, hostname)
def start(self, silent=True):
# start up ROS bridge mode for robot
......@@ -97,9 +99,9 @@ class HVClient(Client):
self.enable_services()
Client.listener.addMsgCallback(self.read)
self.listener.addMsgCallback(self.read)
Client.start_probes(self)
self.start_probes(self)
except Exception as e:
raise e
......@@ -125,9 +127,7 @@ class HVClient(Client):
if len(b) > 1
else list(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.probes)), self.lstnr))
Client.enable_probes(self, probes)
except Exception as e:
......@@ -157,7 +157,7 @@ class HVClient(Client):
try:
name = Client.probe_defs[t][0]
type = Client.probe_defs[t][1]
val = Client.decode_probe_data(type, v)
val = Client.decode_probe_data(self, type, v)
[serv.add(name, val) for serv in self.lstnr]
......
......@@ -9,8 +9,7 @@ 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 hv_client import *
# from libs import *
from libs import *
# Global variables
host_name = "hai-1095.local" # this is the hostname the robot being controlled.
......
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