def main(args):
rospy.init_node('Cameras', anonymous=True)
cams = Cameras()
gate = Gate()
buoy = Buoy()
loc = Localize()
rate = rospy.Rate(30)
lastTime = time() * 1000
try:
while True:
if rospy.is_shutdown():
rospy.logerr("F**K")
break
if (cams.cam0Ready
and (cams.cam1Ready or cams.cameraMap['down'] is None)):
img_gate = np.copy(cams.getFrontFrame())
img_buoy = np.copy(img_gate)
if ((img_gate == np.copy(None)).any() or img_gate is None):
rospy.logwarn("none image recieved")
continue
bars = gate.findBars(img_gate)
loc.updateGate([bars[1], bars[2], bars[3]])
buoys = buoy.mainImg(img_buoy)
img_buoy = buoy.getResultImg()
cams.buoyLinePrediction(
img_buoy,
loc.firstBuoyYaw.getPredictedState()[0])
cams.buoyLinePrediction(
img_buoy,
loc.secondBuoyYaw.getPredictedState()[0],
green=False)
loc.updateBuoy(buoys)
try:
cams.buoyPub.publish(
cams.bridge.cv2_to_imgmsg(img_buoy, "bgr8"))
cams.gatePub.publish(
cams.bridge.cv2_to_imgmsg(img_gate, "bgr8"))
except Exception as e:
rospy.logerr("EXCEPTION IN CAMERAS: ")
rospy.logerr(e)
rospy.logwarn("LOOP TIME %d", time() * 1000 - lastTime)
lastTime = time() * 1000
else:
pass
#print("NOT READY")
except KeyboardInterrupt:
rospy.logerr("Shutting down")
cv2.destroyAllWindows()
rospy.logerr("KILL")
