Commit 4902325c authored by Nicholas Shindler's avatar Nicholas Shindler
Browse files

fixed child classes to handle new format

parent bf45e677
......@@ -15,10 +15,12 @@ class ArmController(Writer):
# Extends the Setter class
###
def __init__(self):
# Create writer for this probe
# manip.armRequestedTargetPositionInPercent
Writer(["manip.armTargetPositionInPercent"])
def run(self, hostname):
#runs a subscriber for setting the arm position.
# runs a subscriber for setting the arm position.
rospy.Subscriber("/" + hostname + "/set_arm_pos", Float32, self.callback)
def callback(self, cmd_msg):
......
......@@ -15,6 +15,8 @@ class GripperController(Writer):
# Extends the Setter class
###
def __init__(self):
# Create writer for this probe
# manip.gripperRequestedTargetPositionInPercent
Writer(["manip.gripperTargetPositionInPercent"])
def callback(self, cmd_msg):
# callback function - extends the setter.callback functionality
......
......@@ -29,13 +29,15 @@ import subprocess
class Client:
def __init__(self):
def __init__(self, host):
try:
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
......
......@@ -15,3 +15,32 @@ ODOM_GYRO_LSTN = 10
FOLLOW_SRV = 11
PICK_SRV = 12
AVOID_SRV = 13
HAS_POT = "manip.botGotPot"
IR_RIGHT_FRONT_A = "bound.rightFront.signalA"
IR_LEFT_FRONT_A = "bound.leftFront.signalA"
IR_RIGHT_BACK_A = "bound.rightBack.signalA"
IR_LEFT_BACK_A = "bound.leftBack.signalA"
IR_RIGHT_FRONT_B = "bound.rightFront.signalB"
IR_LEFT_FRONT_B = "bound.leftFront.signalB"
IR_RIGHT_BACK_B = "bound.rightBack.signalB"
IR_LEFT_BACK_B = "bound.leftBack.signalB"
LOCAL_SCAN = "vis.sick.localScanData"
OBSTACLE_X = "vis.forwardObstacle.x"
OBSTACLE_Y = "vis.forwardObstacle.y"
ODOM_X = "loc.rawOdometry.x"
ODOM_Y = "loc.rawOdometry.y"
ODOM_H = "loc.rawOdometry.h"
ODOM_V = "loc.rawOdometry.v"
ODOM_W = "loc.rawOdometry.w"
ODOM_GRYO_X = "loc.rawOdometryWGyro.x"
ODOM_GYRO_Y = "loc.rawOdometryWGyro.y"
ODOM_GYRO_H = "loc.rawOdometryWGyro.h"
ODOM_GYRO_V = "loc.rawOdometryWGyro.v"
ODOM_GYRO_W = "loc.rawOdometryWGyro.w"
class Reader:
# reader class for getting data from the robot and passing to ROS
def __init__(self, name, p):
def __init__(self, p):
# constructor for settings class
self.probes = p
self.name = name
self.queue = {}
for label in p:
self.queue[label] = 0
def run(self, data):
def pub(self, data):
## Publish data
self.pub.publish(data)
......@@ -17,8 +16,6 @@ class Reader:
try:
if name in self.probes:
self.queue[name] = val
else:
pass
except Exception as e:
raise e
......@@ -28,4 +25,4 @@ class Reader:
def get_probes():
# return an array of all probes
return self.probes
return self.probes if len(self.probes) > 1 else self.probes[0]
......@@ -3,42 +3,33 @@ from std_msgs.msg import Bool
import rospy
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 Getter
from libs.Resources import Reader
from libs.Resources.utils import *
from libs.Resources.config import *
class Gripper(Getter):
class Gripper(Reader):
###
# Gripper information class extending the getter, for handling the publishing of data
###
def init(self, ros, hostname):
self.hostname = hostname
# create publisher
if type(self.name) != tuple:
self.pub = ros.Publisher(
"/" + hostname + "/gripper/" + resource_name(self.name),
Bool,
queue_size=1,
)
else:
self.pub = ros.Publisher(
"/"
+ hostname
+ "/gripper/"
+ resource_name(os.path.commonprefix(self.name)),
Bool,
queue_size=1,
)
# pass to parent to enble probes
Getter.init(self)
# HAS_POT = "manip.botGotPot"
def run(self, q, current_time):
def __init__(self, h):
self.hostname = h
Reader([HAS_POT])
self.pub = rospy.Publisher(
"/" + self.hostname + "/gripper/" + resource_name(self.get_probes()),
Bool,
queue_size=1,
)
def pub(self):
## handles the publishing of data, should be run at an acceptable update interval
# get data from queue
data = Bool(q[self.name])
data = Bool(self.q[HAS_POT])
# publish data
Getter.run(self, data)
Reader.pub(self, data)
......@@ -3,49 +3,48 @@ from std_msgs.msg import Float32
import rospy
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 Getter
from libs.Resources import Reader
from libs.Resources.utils import *
from libs.Resources.config import *
class IrCamera(Getter):
class IrListener(Reader):
###
# IR sensor class extending the getter, for handling the publishing of data
###
def init(self, ros, hostname):
self.hostname = hostname
# create publisher
if type(self.name) != tuple:
self.pub = ros.Publisher(
"/" + hostname + "/ir/" + resource_name(self.name),
Float32,
queue_size=1,
)
else:
self.pub = ros.Publisher(
"/"
+ hostname
+ "/ir/"
+ resource_name(os.path.commonprefix(self.name)),
Float32,
queue_size=1,
)
# pass to parent to enble probes
Getter.init(self)
def run(self, q, current_time):
def __init__(self, h, p):
self.hostname = h
Reader(p)
self.pub = rospy.Publisher(
"/"
+ self.hostname
+ "/gripper/"
+ resource_name(os.path.commonprefix(self.probes)),
Float32,
queue_size=1,
)
def pub(self):
## handles the publishing of data, should be run at an acceptable update interval
# get data from queue
data = []
labels = self.name
if type(labels) != tuple:
labels = labels
for label in labels:
data.append(q[label])
data_list = [self.q[label] for label in self.probes]
data = sum(data_list)/len(data_list)
# publish data
Getter.run(self, data[0])
Reader.pub(self, data)
class IrListenerRightFront(IrListener):
def __init__(self, h):
IrListener(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])
class IrListenerRightBack(IrListener):
def __init__(self, h):
IrListener(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])
......@@ -8,41 +8,38 @@ from geometry_msgs.msg import PointStamped
import sensor_msgs.point_cloud2 as pc2
import math
import time
import ros
import rospy
import struct
import sys, os
import numpy as np
import traceback
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 Getter
from libs.Resources import Reader
from libs.Resources.utils import *
from libs.Resources.config import *
class LidarSensor(Getter):
class LidarListener(Reader):
###
# Lidar sensor class extending the getter, for handling the publishing of data
###
def init(self, ros, hostname):
self.hostname = hostname
# create publisher
def __init__(self, h):
self.hostname = h
Reader([LOCAL_SCAN])
self.pub = ros.Publisher(
"/" + hostname + "/lidar/" + resource_name(self.name),
"/" + hostname + "/lidar/" + resource_name(self.get_probes()),
PointCloud2,
queue_size=1,
)
# pass to parent to enble probes
# self.mp.write_probe(self.codes,1)
Getter.init(self)
def run(self, q, current_time):
def pub(self):
## handles the publishing of data, should be run at an acceptable update interval
# get data from queue
try:
data = q[self.name]
data = self.q[LOCAL_SCAN]
# print data
if type(data) != str:
......@@ -61,96 +58,62 @@ class LidarSensor(Getter):
dat = [
struct.unpack("<f", datStr[i : i + 4]) for i in range(0, len(datStr), 4)
]
# elem_arr = np.zeros((361,3),dtype=float)
elem_arr = []
j = 0
for i in range(0, len(dat), 3):
if len(dat[i:]) < 3:
break
# if j > 360:
# break
elem_arr.append([dat[i], dat[i + 1], 0.05])
z = 0.0
# print "x = %f, y = %f" % (x,y)
# elem_arr[j,0] = dat[i]
# elem_arr[j,1] = dat[i+1]
# j = j + 1
elem_arr = np.array(elem_arr)
header = Header()
header.stamp = current_time
header.stamp = rospy.get_rostime()
header.frame_id = self.hostname
msg = pc2.create_cloud_xyz32(header, elem_arr)
# publish data
Getter.run(self, msg)
Reader.pub(self, msg)
except Exception as e:
# well this is sad
err = traceback.format_exc()
print (err)
# create pointcloud2 message
msg = PointCloud2()
msg.header.stamp = current_time
msg.header.frame_id = self.hostname
# publish data
Getter.run(self, msg)
# enable_probes((3275, 2876,2868,2867,2866,2930))
# enabling 3275 will collect data from laser sensor
class LidarNO(Getter):
class LidarNObsListener(Reader):
###
# Lidar sensor class extending the getter, for handling the publishing of data
# Lidar sensor class to get the nearest object and for handling the publishing of data
###
def init(self, ros, hostname):
self.hostname = hostname
# create publisher
if type(self.name) != tuple:
self.pub = ros.Publisher(
"/" + hostname + "/lidar/" + resource_name(self.name),
PointCloud2,
queue_size=1,
)
else:
self.pub = ros.Publisher(
"/"
+ hostname
+ "/lidar/"
+ resource_name(os.path.commonprefix(self.name)),
PointStamped,
queue_size=1,
)
# pass to parent to enble probes
Getter.init(self)
def run(self, q, current_time):
def __init__(self, h):
self.hostname = h
Reader([OBSTACLE_X,OBSTACLE_Y])
self.pub = ros.Publisher(
"/"
+ hostname
+ "/lidar/"
+ resource_name(os.path.commonprefix(self.get_probes())),
PointStamped,
queue_size=1,
)
def pub(self):
## handles the publishing of data, should be run at an acceptable update interval
# get data from queue
try:
labels = self.name
if len(labels) != 3:
raise Exception("not enough labels")
x = float(q[labels[0]])
y = float(q[labels[1]])
x = float(self.q[OBSTACLE_X])
y = float(self.q[OBSTACLE_Y])
z = 0.0
dist = float(q[labels[2]])
msg = PointStamped()
msg.header.stamp = current_time
msg.header.stamp = rospy.get_rostime()
msg.header.frame_id = self.hostname
msg.point.x = x
msg.point.y = y
msg.point.z = z
# publish data
Getter.run(self, msg)
Reader.pub(self, msg)
except Exception as e:
# well this is sad
......@@ -158,65 +121,3 @@ class LidarNO(Getter):
print (err)
class SickLineSensor(Getter):
###
# Lidar sensor class extending the getter, for handling the publishing of data
###
def init(self, ros, hostname):
self.hostname = hostname
# create publisher
if type(self.name) != tuple:
self.pub = ros.Publisher(
"/" + hostname + "/sick/" + resource_name(self.name),
PointCloud2,
queue_size=1,
)
else:
self.pub = ros.Publisher(
"/"
+ hostname
+ "/sick/"
+ resource_name(os.path.commonprefix(self.name)),
PointCloud2,
queue_size=1,
)
# pass to parent to enble probes
Getter.init(self)
self.dat = []
def run(self, q, current_time):
## handles the publishing of data, should be run at an acceptable update interval
# get data from queue
print "WHY IS THIS RUNNING!!!"
try:
x = 0
y = 0
labels = self.name
if len(labels) != 2:
raise Exception("Not enough labels!")
x = float(q[labels[0]])
y = float(q[labels[1]])
self.dat.append([x, y, 0.05])
if len(self.dat) > 270:
N = len(self.dat) - 270
self.dat = self.dat[N:]
xyz = np.array(self.dat)
header = Header()
header.stamp = current_time
header.frame_id = self.hostname
msg = pc2.create_cloud_xyz32(header, xyz)
# publish data
Getter.run(self, msg)
except Exception as e:
# well this is sad
err = traceback.format_exc()
print (err)
......@@ -13,60 +13,54 @@ import rospy
import tf
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 Getter
from libs.Resources import Reader
from libs.Resources.utils import *
from libs.Resources.config import *
class Odometer(Getter):
class Odometer(Reader):
###
# Odometer class extending the getter, for handling the publishing of data
# Odometer class for reading the positional state data and for handling the publishing of data
###
def init(self, ros, hostname):
self.hostname = hostname
# create publisher
if type(self.name) != tuple:
self.pub = ros.Publisher(
"/" + hostname + "/odom/" + resource_name(self.name),
Odometry,
queue_size=1,
)
else:
self.pub = ros.Publisher(
"/"
+ hostname
+ "/odom/"
+ resource_name(os.path.commonprefix(self.name)),
Odometry,
queue_size=1,
)
# pass to parent to enble probes
Getter.init(self)
def run(self, q, current_time):
def __init__(self, h, p):
self.hostname = h
Reader(self,p)
self.pub = ros.Publisher(
"/"
+ hostname
+ "/odom/"
+ resource_name(os.path.commonprefix(self.get_probes())),
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 = current_time
odom.header.stamp = rospy.get_rostime()
odom.header.frame_id = self.hostname
data = []
labels = self.name
if type(labels) != tuple:
labels = labels
for label in labels:
data.append(q[label])
heading = tf.transformations.quaternion_from_euler(0.0, 0.0, data[2])
[X,Y,H,V,W] = list(map(self.q.get,self.get_probes()))
heading = tf.transformations.quaternion_from_euler(0.0, 0.0, H)
odom.pose.pose = Pose(
Point(data[0], data[1], 0.0),
Point(X, Y, 0.0),
Quaternion(heading[0], heading[1], heading[2], heading[3]),
)
odom.twist.twist = Twist(Vector3(data[3], 0, 0), Vector3(0, 0, data[4]))
odom.twist.twist = Twist(Vector3(V, 0, 0), Vector3(0, 0, W))
# publish data
Getter.run(self, odom)
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])
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])
......@@ -25,7 +25,7 @@ from libs import *
class HVClient(Client):
def __init__(self, services):
def __init__(self, hostname, services):
try:
# enable services, selection default is all (0)
self.cmdr = []
......@@ -34,35 +34,38 @@ class HVClient(Client):
if DRIVE_CMD in services:
# enable drive controller
self.cmdr.append(DriveController())
self.cmdr.append(DriveController(hostname))
if GRIPPER_CMD in services: