def main():
# Camera setup
camera_stack = []
camera_n = Semaphore(0)
camera_s = Semaphore(1)
cameras = Camera(camera_stack, camera_n, camera_s)
# Start the thread
cameras.start()
# Try to pull from the stack every 1 second and print data
for i in range(0, 10):
time.sleep(1)
camera_n.acquire()
camera_s.acquire()
left_lines = camera_stack.pop()
right_lines = camera_stack.pop()
camera_s.release()
print(i)
if right_lines != None:
for line in right_lines:
for rho, theta in line:
print(rho, theta)
else:
print("No right lines")
if left_lines != None:
for line in left_lines:
for rho, theta in line:
print(rho, theta)
else:
print("No left lines")
# Stop camera thread and wait for it to finish
cameras.stop()
cameras.join()
class HeliosController(Observable):
md = None
ds = None
acl = None
cam = None
status = {}
def __init__(self, conffile):
Observable.__init__(self)
cp = ConfigParser()
cp.read(conffile)
if not Globals.globSimulate:
wiringpi.wiringPiSetupGpio()
self.md = MotorDriver([
cp.getint("MotorDriver", "LEFT_L"),
cp.getint("MotorDriver", "LEFT_R"),
cp.getint("MotorDriver", "RIGHT_L"),
cp.getint("MotorDriver", "RIGHT_R"),
cp.getint("MotorDriver", "VERTICAL_L"),
cp.getint("MotorDriver", "VERTICAL_R"),
])
self.md.start()
self.ds = DistanceSensor(
cp.getint("DistanceSensor", "TRIGGER"),
cp.getint("DistanceSensor", "ECHO")
)
self.ds.start()
self.acl = AltitudeControlLoop(self.md, self.ds)
self.update("MotorDriver", self.md)
self.update("DistanceSensor", self.ds)
self.update("AltitudeControlLoop", self.acl)
self.md.subscribe(self.update)
self.ds.subscribe(self.update)
self.acl.subscribe(self.update)
self.setCamera(False)
#self.acl.debug = True
#self.md.debug = True
def shutdown(self):
self.md.shutdown()
self.md.join()
self.ds.shutdown()
self.ds.join()
self.setCamera(False)
def turnLeft(self):
self.md.turnLeft()
def turnRight(self):
self.md.turnRight()
def forward(self):
self.md.forward()
def backward(self):
self.md.backward()
def up(self):
self.md.up()
def down(self):
self.md.down()
def setSpeed(self, val):
self.md.setSpeed(val)
def setAuto(self, val):
self.acl.setAuto(val)
def setHeight(self, val):
self.acl.setHeight(val)
def setForceDescent(self, val):
self.acl.setForceDescent(val)
def setSingleSteerMode(self, val):
self.md.setSingleSteerMode(val)
def setCamera(self, val):
if self.cam == None:
if val:
print "HeliosController enabling camera"
self.cam = Camera()
self.cam.subscribe(self.updateImage)
self.cam.start()
else:
print "HeliosController tried to disable camera, but is not active"
else:
if val:
print "HeliosController tried to enable camera, but is already active"
else:
print "HeliosController disabling camera"
self.cam.shutdown()
self.cam.join()
self.cam = None
self.status["cameraImage"] = "data:image/jpeg;base64," + base64.b64encode(open("nocam.jpg","rb").read())
self.status["cameraActive"] = True if self.cam != None else False
self.emit("HeliosController", self)
def updateImage(self, id, cam):
self.status["cameraImage"] = "data:image/jpeg;base64," + base64.b64encode(cam.getImage())
def getImage(self):
if self.cam != None:
return self.cam.getImage()
return open("nocam.jpg","rb").read()
def update(self, etype, src):
if etype == "MotorDriver":
self.status["statLeft"] = src.statusLeft()
self.status["statRight"] = src.statusRight()
self.status["statVert"] = src.statusVert()
self.status["pwmValue"] = src.getPWM()
self.status["singleSteeringMode"] = src.getSingleSteerMode()
if etype == "DistanceSensor":
self.status["currentAltitude"] = src.getCurrentDistance()
self.status["altitudeHistory"] = src.getDistanceHistory()
if etype == "AltitudeControlLoop":
self.status["requestedAltitude"] = src.getHeight()
self.status["maintainAltitude"] = src.getAuto()
self.status["forceDescent"] = src.getForceDescent()
self.emit("HeliosController", self)
def getStatus(self):
return self.status
def main():
logger.debug("Starting TCNJ IGVC 2017")
logger.debug("Getting USB device mappings")
device_to_path = get_device_paths()
# GPS setup
logger.debug("Beginning GPS setup")
gps_coords_stack = Manager().list()
gps_n = Semaphore(0)
gps_s = Semaphore(1)
gps_sensor = GPS(gps_coords_stack, gps_n, gps_s, device_to_path["GPS"])
logger.debug("GPS setup complete")
# LiDAR setup
logger.debug("Beginning LiDAR setup")
lidar_data_stack = Manager().list()
lidar_n = Semaphore(0)
lidar_s = Semaphore(1)
lidar_sensor = LIDAR(lidar_data_stack, lidar_n, lidar_s, device_to_path["LIDAR"])
logger.debug("LiDAR setup complete")
# Camera setup
logger.debug("Beginning camera setup")
camera_lines_stack = Manager().list()
camera_n = Semaphore(0)
camera_s = Semaphore(1)
camera_controller = Camera(camera_lines_stack, camera_n, camera_s, device_to_path["RIGHT_CAM"], device_to_path["LEFT_CAM"])
logger.debug("Camera setup complete")
# Compass setup
logger.debug("Beginning compass setup")
compass_stack = Manager().list()
compass_n = Semaphore(0)
compass_s = Semaphore(1)
compass = Compass(compass_stack, compass_n, compass_s)
logger.debug("Compass setup complete")
# Wrap all the sensors' stacks and semaphores into 1 object
sensors = Sensors(
gps_coords_stack, gps_n, gps_s,
lidar_data_stack, lidar_n, lidar_s,
camera_lines_stack, camera_n, camera_s,
compass_stack, compass_n, compass_s
)
# Start the sensor processes
logger.debug("Setup complete")
logger.debug("Starting GPS process")
gps_sensor.start()
logger.debug("Startting LiDAR process")
lidar_sensor.start()
logger.debug("Starting camera process")
camera_controller.start();
logger.debug("Starting compass process")
compass.start();
# Set up wiringpi2 and GPIO pin 6 as input
logger.debug("Sensor process started, waiting for motors to turn on")
motors_on_pin = 6
wpi.wiringPiSetup()
wpi.pinMode(motors_on_pin, 0)
# Wait for motor controller to be turned on
motors_on = False
while not motors_on:
value = wpi.digitalRead(motors_on_pin)
if value == 1:
motors_on = True
else:
time.sleep(1)
logger.debug("Motors are on, begin autonomous navigation")
path_find = Avoidance(sensors)
path_find.start()
time.sleep(10)
# Stop the processes
logger.debug("Calling for processes to stop")
path_find.stop()
time.sleep(1)
gps_sensor.stop()
lidar_sensor.stop()
camera_controller.stop()
compass.stop()
# Clean up the processes
logger.debug("Waiting for processes to end")
path_find.join()
gps_sensor.join()
lidar_sensor.join()
camera_controller.join()
compass.join()
print("--------------- CAMERA STACK ---------------")
print(camera_lines_stack)
print("--------------- GPS STACK ---------------")
print(gps_coords_stack)
print("--------------- LIDAR STACK ---------------")
print(lidar_data_stack)
print("--------------- COMPASS STACK ---------------")
print(compass_stack)
print("--------------- DONE ---------------")
