def __init__(self):
super().__init__()
self.title = "PyQt5 simple window - pythonspot.com"
self.left = 10
self.top = 10
self.width = 640
self.height = 480
self.msg = example_t()
self.msg.mode = 0
self.lc = lcm.LCM()
self.initUI()
self.showMaximized()
def send(lc=lc, textbox=textbox):
global last_sent, last_sent_msg
msg = example_t()
msg.timestamp = 0
msg.position = (1, 2, 3)
msg.orientation = (1, 0, 0, 0)
msg.ranges = range(15)
msg.num_ranges = len(msg.ranges)
msg.name = textbox.text
msg.enabled = True
lc.publish("TO_ARDUINO", msg.encode())
last_sent = time.time()
last_sent_msg = textbox.text
print "Message Sent"
def changeColor(self):
source = self.sender()
msg = example_t()
if source.text() == "移動":
msg.mode = 0
self.lc.publish("EXAMPLE", msg.encode())
self.btn1.setStyleSheet('QPushButton {background-color: #00ff00}')
self.btn2.setStyleSheet('QPushButton {background-color: #AAAAAA}')
self.btn3.setStyleSheet('QPushButton {background-color: #AAAAAA}')
elif source.text() == "リフトアップ":
msg.mode = 1
self.lc.publish("EXAMPLE", msg.encode())
self.btn1.setStyleSheet('QPushButton {background-color: #AAAAAA}')
self.btn2.setStyleSheet('QPushButton {background-color: #00ff00}')
self.btn3.setStyleSheet('QPushButton {background-color: #AAAAAA}')
elif source.text() == "アーム操作":
msg.mode = 2
self.lc.publish("EXAMPLE", msg.encode())
self.btn1.setStyleSheet('QPushButton {background-color: #AAAAAA}')
self.btn2.setStyleSheet('QPushButton {background-color: #AAAAAA}')
self.btn3.setStyleSheet('QPushButton {background-color: #00ff00}')
import lcm
from exlcm import example_t
##################################### LCM #########################################################
######publishされたら動く############
def my_handler(channel, data):
global msg
msg = example_t.decode(data)
msg.R_list=list(msg.R_list)
def subscribe_handler(handle):
while True:
handle()
msg = example_t()
lc = lcm.LCM()
subscription = lc.subscribe("EXAMPLE", my_handler)
########handleをwhileでぶん回すのをサブスレッドで行う############
thread1 = threading.Thread(target=subscribe_handler, args=(lc.handle,))
thread1.setDaemon(True)
thread1.start()
###########################################################################################################
(w,h) = (400,400) # 画面サイズ
(x,y) = (w/2, h/2)
pygame.init() # pygame初期化
pygame.display.set_mode((w, h), 0, 32) # 画面設定
screen = pygame.display.get_surface()
import lcm
import time
from exlcm import example_t
lc = lcm.LCM()
msg = example_t()
msg.timestamp = int(time.time() * 1000000)
msg.position = (1, 2, 3)
msg.orientation = (1, 0, 0, 0)
msg.ranges = range(15)
msg.num_ranges = len(msg.ranges)
msg.name = "example string"
msg.enabled = True
lc.publish("EXAMPLE", msg.encode())
def main():
global calibrateMode
global turnMode
lc = lcm.LCM() # Create object of lcm library
subscription = lc.subscribe("PROCESSING_RECEIVE", msg_handler)
print "Getting pyfly2 context ..."
context = pyfly2.Context()
print "Done."
# assert(not (context.num_cameras == 0))
print "Getting camera ...",
camera = context.get_camera(0)
print "Done."
print "Connecting to camera ...",
camera.Connect()
print "Done."
print "Querying camera information ..."
for k, v in camera.info.iteritems():
print k, v
print "Done."
print "Starting capture mode ...",
camera.StartCapture()
print "Done."
start_time = time.time()
counter = 0
turnAngle_Ref = mc.getYaw()
print "Reference angle:", turnAngle_Ref
while (True):
end_time = time.time()
if (end_time - start_time > PROCESSING_INTERVAL):
stopBot() # Stop the bot during processing
frame = camera.GrabNumPyImage('bgr')
# TODO: Undistort frame
cv2.imwrite("share/" + str(counter) + ".jpg", frame)
print "Written to share/" + str(counter) + ".jpg"
msg = example_t()
msg.currImage = counter
# TODO add focal length to message / result
lc.publish("PROCESSING_SEND", msg.encode())
print "published by gc.py"
# TODO Delete previous image
lc.handle() # Recieve angle
start_time = time.time() # Restart count
counter += 1
global turnAngle
ANGLE_THRES = 0.13
angle_to_rotate = turnAngle
print "Angle to rotate:", angle_to_rotate
if (abs(angle_to_rotate) >
ANGLE_THRES): # Turn bot (sharply) in given direction
rotate(angle_to_rotate * 180 / math.pi)
else:
moveForward()
# TODO handle bot movement
time.sleep(SLEEP_TIME)
def main():
counter = int(raw_input())
lc = lcm.LCM() # Create object of lcm library
msg = example_t()
msg.currImage = counter
lc.publish("PROCESSING_SEND", msg.encode())
