class ExamplePub():
def __init__(self):
self.name = "examplepub"
self.gunTurret = GunTurret(0, 0, 40, [-100, 50, 100])
self.Comms = Comms()
self.Comms.add_publisher_port('127.0.0.1', '3003', 'gunPath')
def sendPath(self, path):
self.Comms.define_and_send(self.name, 'gunPath', path)
def run(self, is_sim):
"""
Check subscriber bus for path matrix. If path is found, clear cameraPath and replace with new path.
If no path is found,
"""
while (True):
q1, q2, toa, f = self.gunTurret.inverseKin()
q_mat = self.gunTurret.scurvePath(
np.array([0, 0]).reshape(2, 1),
np.array([q1, q2]).reshape(2, 1), 6, 1.5, .05)
self.sendPath(q_mat)
time.sleep(3)
class ExampleCameraPub():
def __init__(self):
self.name = "examplecamera"
self.cameraTurret = CameraTurret()
self.Comms = Comms()
self.Comms.add_publisher_port('127.0.0.1', '3002', 'cameraPath')
def sendPath(self, path):
self.Comms.define_and_send(self.name, 'cameraPath', path)
def run(self, is_sim):
"""
Check subscriber bus for path matrix. If path is found, clear cameraPath and replace with new path.
If no path is found,
"""
# Testing required to find q1_max and q2_max
while (True):
q_mat = self.cameraTurret.sweepPath(5,
q1_range=(-np.pi / 4,
np.pi / 4),
q2_range=(-np.pi / 4,
np.pi / 4))
self.sendPath(q_mat)
time.sleep(5)
class CameraInterface():
def __init__(self):
self.name = "camera"
# Keep track of all targets seen to ensure only one fwdkin is pushed downstream for each
self.target_log = set()
# Targets defined here for simulation only
self.cTurret = CameraTurret([
Target(np.array([0, 100, 100]).reshape(3, 1), "t0"),
Target(np.array([0, 100, 150]).reshape(3, 1), "t1"),
Target(np.array([0, 150, 150]).reshape(3, 1), "t2")
])
self.Comms = Comms()
self.Comms.add_subscriber_port('127.0.0.1', '3000', 'cState')
self.Comms.add_publisher_port('127.0.0.1', '3004', 'targetPos')
# twenty frames per second
self.frame_delay = .05
def getCState(self):
try:
camera_pos = self.Comms.get('cState').payload
self.cTurret = CameraTurret([
Target(np.array([0, 100, 100]).reshape(3, 1), "t0"),
Target(np.array([0, 100, 150]).reshape(3, 1), "t1"),
Target(np.array([0, 150, 150]).reshape(3, 1), "t2")
], camera_pos[0][0], camera_pos[1][0])
except queue.Empty:
pass
def publishPos(self, id, target):
"""
Publishes target coordinates and target id in origin frame to pathing algorithms
"""
self.Comms.define_and_send(id, 'targetPos', target)
def runS(self):
prev_frame = time.time()
while (True):
# If a time delay has passed, grab a frame (in simulation, do nothing)
if (time.time() - prev_frame >= self.frame_delay):
prev_frame = time.time()
# Update camera turret coords
self.getCState()
# run marker detection on frame (or for simulation, check for targets in range)
targets = self.cTurret.targetsInView()
# Get new targets and add to log set
new_targets = {
key: targets[key]
for key in (targets.keys() - self.target_log)
}
self.target_log = self.target_log | new_targets.keys()
# if marker detected (or target is within view range for simulation), compute forward kinematics.
# for simulation, first derive target in camera frame, then run forward kinematics
# on target and publish with topic set to marker id.
R01 = zRot(self.cTurret.q1_given)
R12 = yRot(self.cTurret.q2_given)
p_1 = self.cTurret.orig + R01 @ self.cTurret.p12
p_2 = p_1 + R01 @ R12 @ self.cTurret.pOffset
t_links = self.cTurret.getTargetLinks(p_2, new_targets.values())
for (t_link, target) in zip(t_links, new_targets.keys()):
targ_pos = self.cTurret.representTarget(t_link)
self.publishPos(target, targ_pos)
print(targ_pos)
time.sleep(.02)
# TODO perform opencv marker detection
def runR(self):
# Get marker dictionary set
marker_dict = aruco.Dictionary_get(aruco.DICT_5X5_1000)
# Get default detector parameters
d_params = aruco.DetectorParameters_create()
# get camera parameters
cameraMatrix = np.array([])
distCoeffs = np.array([])
with open('cameraData.json', 'r') as cameraData:
data = json.load(cameraData)
for camera in data['cameras']:
if camera['name'] == 'Sahith Mac':
parameters = camera['parameters']
cameraMatrix = np.array([
parameters['lw'], 0, parameters['u0'], 0,
parameters['lh'], parameters['v0'], 0, 0, 1
]).reshape(3, 3)
distCoeffs = np.array([parameters['distortions']])
# Set up videocap stream
cap = cv2.VideoCapture(0)
# time previous frame was captured
prev_frame = time.time()
while (True):
targets = None
target_ids = None
# If a time delay has passed, grab a frame (in simulation, do nothing)
if (time.time() - prev_frame >= self.frame_delay):
# grab a frame from videocap
ret, frame = cap.read()
# Detect markers and corners in frame
corners, target_ids, rejected = aruco.detectMarkers(
frame, marker_dict, parameters=d_params)
# Estimate marker pose
_, targets, _ = aruco.estimatePoseSingleMarkers(
corners, 0.05, cameraMatrix, distCoeffs)
prev_frame = time.time()
# Update camera turret coords
self.getCState()
if targets is None:
continue
# Get new targets and add to log set
new_ids = set(target_ids.flatten()) - self.target_log
self.target_log = self.target_log | set(target_ids.flatten())
#new_targets = [targets for i in zip(target_ids, targets)]
# if marker detected, compute forward kinematics.
for (t_link, id) in zip(targets, target_ids):
if id not in list(new_ids):
continue
targ_pos = np.array([[-1, 1, 1]]).T * zRot(-np.pi / 2) @ xRot(
-np.pi / 2) @ self.cTurret.representTarget(1000 * t_link.T)
self.publishPos(id, targ_pos)
print("Targ {}:".format(id))
print(targ_pos)
print()
time.sleep(.05)
def run(self, is_sim):
if is_sim:
self.runS()
else:
self.runR()
class CameraMotion():
def __init__(self):
self.name = "camera"
self.cameraTurret = CameraTurret()
self.currentPos = np.array([0, 0]).reshape(2, 1)
self.currentTarget = np.array([])
self.gunPath = np.array([])
self.Comms = Comms()
self.Comms.add_subscriber_port('127.0.0.1', '3000', 'cState')
self.Comms.add_publisher_port('127.0.0.1', '3002', 'cameraPath')
self.Comms.add_subscriber_port('127.0.0.1', '3004', 'targetPos')
def sendPath(self, path):
self.Comms.define_and_send(self.name, 'cameraPath', path)
def receive(self):
try:
self.currentTarget = self.Comms.get('targetPos').payload
except queue.Empty:
pass
try:
self.currentPos = self.Comms.get('cState').payload
#print("new state: ", self.currentPos)
except queue.Empty:
pass
def getFullSweep(self):
duration = 5
# Testing required to find q1_range and q2_range
# Eventually should be declared as constants
q_mat = self.cameraTurret.sweepPath(duration,
q1_range=(-np.pi / 4, np.pi / 4),
q2_range=(-np.pi / 4, np.pi / 4))
# Make duplicate for reverse direction
q_mat2 = self.cameraTurret.sweepPath(duration,
q1_range=(-np.pi / 4, np.pi / 4),
q2_range=(-np.pi / 4, np.pi / 4))
q_mat2 = np.flip(q_mat2, 1)[:, 1:]
q_size = int((q_mat2.size) / 3)
# Change timestamps of reverse array
for i in range(q_size):
q_mat2[0, i] = duration + (duration - q_mat2[0, i])
return np.concatenate((q_mat, q_mat2), axis=1), duration * 2
def runS(self):
sweepPath, duration = self.getFullSweep()
sweepInit = np.array(sweepPath[1:3, 0]).reshape(2, 1)
print(sweepInit)
self.sendPath(sweepPath)
startTime = time.time()
#print(path, duration)
# self.currentPos is incorrect, fixes should happen in hardware_interface
while (True):
self.receive()
if self.currentTarget.size:
targetPos = self.cameraTurret.inverseKin(self.currentTarget)
targetPath = self.cameraTurret.scurvePath(
self.currentPos, targetPos, 10, 3, 0.1)
#print(self.currentPos)
self.sendPath(targetPath)
# Hardcoded to duration of scurve * 2, may need to switch to when gun shoots (another connection)
time.sleep(6)
self.currentTarget = np.array([])
else:
if time.time() - startTime >= duration * 1.5:
# Destination is start of sweep path, hardcoded to (-np.pi/4, -np.pi/4)
# because sweep path init is hardcoded to this point
self.sendPath(
self.cameraTurret.scurvePath(self.currentPos,
sweepInit, 10, 2, 0.1))
# Hardcoded to duration of scurve * 2
time.sleep(4)
self.sendPath(sweepPath)
startTime = time.time()
# Estimate of how long simulation actually takes vs expected duration
# Need to test to get accurate scale
# Also add conditional to see if target is found before duration is over
time.sleep(0.02)
def runSweep(self):
sweepPath, duration = self.getFullSweep()
startTime = 0
while (True):
if time.time() - startTime >= duration * 1.4:
self.sendPath(sweepPath)
startTime = time.time()
time.sleep(0.02)
def runR(self):
pass
def run(self, is_sim):
if is_sim:
# Change this function to run sweep only vs full motion mode
self.runSweep()
else:
self.runR()
class HardwareInterface():
def __init__(self):
self.name = "hardware"
self.gun_config = np.array([[0, 0]]).T
self.camera_config = np.array([[0, 0]]).T
self.gun_ready = 0
self.gunReady = True
self.gunTurretReady = False
# When a target has been detected and
self.cameraPath = None
self.gunPath = None
# Simulation graphics engine
self.sim_out = Engine(
np.array([150, 150, 150]).reshape(3, 1),
np.array([np.pi / 2 - .4, -1, .3]).reshape(3, 1))
self.Comms = Comms()
self.Comms.add_publisher_port('127.0.0.1', '3000', 'cState')
self.Comms.add_publisher_port('127.0.0.1', '3001', 'gState')
self.Comms.add_subscriber_port('127.0.0.1', '3002', 'cameraPath')
self.Comms.add_subscriber_port('127.0.0.1', '3003', 'gunPath')
def initSerial(self):
# Establish serial port and baud rate
self.ard = serial.Serial('COM3', 9600)
if (self.ard.isOpen() == False):
self.ard.open()
def readStateR(self):
line = []
while True:
for c in self.ard.read():
if chr(c) == ':':
# parse return from arduino
#print("".join(line))
# Run comma delimited parse on [2:]
parsed = "".join(line[2:]).split(',')
if (line[0] == 'c'):
self.camera_config[0][0] = float(parsed[0])
self.camera_config[1][0] = float(parsed[1])
elif (line[0] == 'g'):
self.gun_config[0][0] = float(parsed[0])
self.gun_config[1][0] = float(parsed[1])
elif (line[0] == 's'):
self.gun_ready = float(parsed[0])
"""
print(camera_config)
print(gun_config)
print(gun_ready)
"""
line = []
break
else:
line.append(chr(c))
def readStateS(self):
"""
Reads servo configurations and updates internal state (simulation)
"""
# Do nothing, since ideal situation has interface state always equal to actual hardware state
self.camera_config = self.camera_config
self.gun_config = self.gun_config
def writeCamR(self, ctarg):
"""
Writes a servo configuration (hardware)
"""
self.ard.write("c,{},{}:".format(ctarg[0][0], ctarg[1][0]).encode())
def writeCamS(self, ctarg):
"""
Writes a servo configuration (simulation)
"""
# Updates engine with new configurations and draws
self.sim_out.clearGeometries()
self.camera_config = ctarg
# Grid and target dimensions are hardcoded
self.drawSim(ctarg, self.gun_config)
#self.sim_out.update()
def writeGunR(self, gtarg):
self.ard.write("g,{},{}:".format(gtarg[0][0], gtarg[1][0]).encode())
def writeGunS(self, gtarg):
"""
Writes a servo configuration (simulation)
"""
# Updates engine with new configurations and draws
self.sim_out.clearGeometries()
self.gun_config = gtarg
# Grid and target dimensions are hardcoded
self.drawSim(self.camera_config, gtarg)
# self.sim_out.update()
def writeShootR(self):
self.ard.write("s:".encode())
def publishState(self):
"""
Publishes current configuration states of gun turret and camera turret to both publisher ports
"""
self.Comms.define_and_send(self.name, 'cState', self.camera_config)
self.Comms.define_and_send(self.name, 'gState', self.gun_config)
def receivePath(self):
"""
Pulls path matrices from subscriber bus and replace current path matrices
"""
try:
self.cameraPath = self.Comms.get('cameraPath').payload
except queue.Empty:
pass
try:
self.gunPath = self.Comms.get('gunPath').payload
except queue.Empty:
pass
def drawSim(self, cameraPos, gunPos):
self.sim_out.addGeometry([Grid(20, 20)])
self.sim_out.addGeometry([
Target(np.array([0, 100, 100]).reshape(3, 1), "t0"),
Target(np.array([0, 100, 150]).reshape(3, 1), "t1"),
Target(np.array([0, 150, 150]).reshape(3, 1), "t2")
])
self.sim_out.addGeometry([
CameraTurret([
Target(np.array([0, 100, 100]).reshape(3, 1), "t0"),
Target(np.array([0, 100, 150]).reshape(3, 1), "t1"),
Target(np.array([0, 150, 150]).reshape(3, 1), "t2")
], cameraPos[0, 0], cameraPos[1, 0])
])
self.sim_out.addGeometry(
[GunTurret(gunPos[0, 0], gunPos[1, 0], 70, [-100, 50, 100])])
# def fireTheShot(self):
# # TODO: Send a fire command to the gun to Arduino and start reloading
# self.gunReady = False
# def checkTurretReady(self):
# """
# Check if the turret is aiming at the target
# """
# error = 0.02
# if self.gunPath[-1][0] * (1 - error) < self.gun_config[0] < self.gunPath[-1][0] * (1 + error) and \
# self.gunPath[-1][1] * (1 - error) < self.gun_config[1] < self.gunPath[-1][1] * (1 + error):
# self.gunReady = True
# else:
# self.gunReady = False
def runS(self):
"""
Check subscriber bus for path matrix. If path is found, clear cameraPath and replace with new path.
If no path is found,
"""
prev_gun_write = time.time()
gun_write_delay = 0
prev_camera_write = time.time()
camera_write_delay = 0
self.drawSim(self.camera_config, self.gun_config)
while (True):
# Update configuration states and publish them
self.readStateS()
self.publishState()
#self.checkTurretReady()
# Pull path matrix from queue and replace existing matrices
self.receivePath()
# If path steps exist and sufficient time has elapsed since the last write, write again
if self.gunPath is not None and self.gunPath.shape[1] != 0 and\
time.time() - prev_gun_write >= gun_write_delay:
# Write config
self.writeGunS(self.gunPath[1:3, 0:1])
prev_gun_write = time.time()
# Set new delay
if self.gunPath.shape[1] > 1:
# subtract next timestamp from current timestamp for delay
gun_write_delay = self.gunPath[0][1] - self.gunPath[0][0]
else:
gun_write_delay = 0
# Pop current config
self.gunPath = self.gunPath[0:3, 1:]
if self.cameraPath is not None and self.cameraPath.shape[1] != 0 and\
time.time() - prev_camera_write >= camera_write_delay:
# Write config
self.writeCamS(self.cameraPath[1:3, 0:1])
prev_camera_write = time.time()
# Set new delay
if self.cameraPath.shape[1] > 1:
# subtract next timestamp from current timestamp for delay
camera_write_delay = self.cameraPath[0][
1] - self.cameraPath[0][0]
else:
camera_write_delay = 0
# Pop current config
self.cameraPath = self.cameraPath[0:3, 1:]
# if self.gunReady and self.gunTurretReady:
# self.fireTheShot()
# Display simulation.
self.sim_out.update()
# Hardware interface updates 50 times a second
time.sleep(.02)
def runR(self):
"""
Check subscriber bus for path matrix. If path is found, clear cameraPath and replace with new path.
If no path is found,
"""
self.initSerial()
prev_gun_write = time.time()
gun_write_delay = 0
prev_camera_write = time.time()
camera_write_delay = 0
t = threading.Thread(target=self.readStateR(), args=())
t.start()
while (True):
# Update configuration states and publish them
self.readStateR()
self.publishState()
# Pull path matrix from queue and replace existing matrices
self.receivePath()
# If path steps exist and sufficient time has elapsed since the last write, write again
if (self.gunPath is not None) and\
(self.gunPath.shape[1] != 0) and\
(time.time() - prev_gun_write >= gun_write_delay):
# Write config
self.writeGunR(self.gunPath[1:3, 0:1])
prev_gun_write = time.time()
# Set new delay
if (self.gunPath.shape[1] > 1):
# subtract next timestamp from current timestamp for delay
gun_write_delay = self.gunPath[0][1] - self.gunPath[0][0]
else:
gun_write_delay = 0
# Pop current config
self.gunPath = self.gunPath[0:3, 1:]
if (self.cameraPath is not None) and\
(self.cameraPath.shape[1] != 0) and\
(time.time() - prev_camera_write >= camera_write_delay):
# Write config
self.writeCamR(self.cameraPath[1:3, 0:1])
prev_camera_write = time.time()
# Set new delay
if (self.cameraPath.shape[1] > 1):
# subtract next timestamp from current timestamp for delay
camera_write_delay = self.cameraPath[0][
1] - self.cameraPath[0][0]
else:
camera_write_delay = 0
# Pop current config
self.cameraPath = self.cameraPath[0:3, 1:]
# Hardware interface updates 50 times a second
time.sleep(.02)
def run(self, is_sim):
if is_sim:
self.runS()
else:
self.runR()
class GunMotion():
def __init__(self):
self.name = "gunmotion"
# Current configuration of gun turret as reported by hardware
self.actual_configuration = np.array([[0, 0]]).T
self.dest_configuration = np.array([[0, 0]]).T
# Configuration comparison tolerance (rad)
self.tol = .025
# First target indicator to skip delay
self.first_targ = True
# Time to wait for gun to fire
self.delay = 2
# queue of P0Ts to process
self.targets = []
self.gTurret = GunTurret(0, 0, 500, [-100, 50, 100])
self.Comms = Comms()
self.Comms.add_subscriber_port('127.0.0.1', '3001', 'gState')
self.Comms.add_subscriber_port('127.0.0.1', '3004', 'targetPos')
self.Comms.add_publisher_port('127.0.0.1', '3003', 'gunPath')
def receive(self):
"""
Reads gun turret configurations and the target position
"""
try:
self.actual_configuration = self.Comms.get('gState').payload
except queue.Empty:
pass
try:
self.targets.append(self.Comms.get('targetPos').payload)
except queue.Empty:
pass
def publish(self, pathMatrix):
"""
Publishes the gun path
"""
self.Comms.define_and_send(self.name, 'gunPath', pathMatrix)
def compareTol(self):
# If there is a destination, see if configs match
#for q, q_prime in zip(self.actual_configuration, self.dest_configuration):
for i in range(self.actual_configuration.shape[0]):
if (self.actual_configuration[i][0] >
self.dest_configuration[i][0] + self.tol
or self.actual_configuration[i][0] <
self.dest_configuration[i][0] - self.tol):
return False
# Delay for as long as the gun takes to wind up and shoot
# For real demo, about 6 seconds
if self.first_targ:
self.first_targ = False
else:
time.sleep(self.delay)
# Allow another path matrix to be sent
return True
def run(self, is_sim):
"""
Checks the configuration of gun turret and the target, if a target is received, calculate the path matrix and
delete the target to wait for the next one
"""
while True:
self.receive()
"""
if self.prev_path_time is not None and time.time() > self.prev_path_time + self.delay_time:
self.prev_path_time = None
continue
"""
if self.compareTol() and len(self.targets) > 0:
self.gTurret.setP0T(self.targets.pop(0))
q1, q2, toa, f = self.gTurret.inverseKin(print_time=True)
pathMatrix = self.gTurret.scurvePath(
self.actual_configuration,
np.array([q1, q2]).reshape(2, 1), 10, 1.5, .05)
self.dest_configuration = np.array([[
pathMatrix[1][pathMatrix.shape[1] - 1],
pathMatrix[2][pathMatrix.shape[1] - 1]
]]).T
self.publish(pathMatrix)
time.sleep(.02)
