class HQControlServer():
def __init__(self):
self.controller = Controller()
self.coef = 58823530
self.port = "50060"
if len(sys.argv) > 1:
self.port = sys.argv[1]
int(self.port)
self.context = zmq.Context()
self.socket = self.context.socket(zmq.REP)
self.socket.bind("tcp://*:%s" % self.port)
def listen_commands(self):
print "server is ready for commands"
while True:
# Wait for next request from client
message = self.socket.recv()
direction, velocity = json.loads(message)
if direction == 'forward':
self.handleButtonFWD(velocity)
if direction == 'back':
self.handleButtonBack(velocity)
if direction == 'left':
self.handleButtonL(velocity)
if direction == 'right':
self.handleButtonR(velocity)
if direction == 'stop':
self.handleButtonSTP()
self.socket.send("OK")
def from_real_to_drive_vel(self, real):
return float(real) * self.coef
def handleButtonFWD(self, vel):
self.controller.set_global_velocity(self.from_real_to_drive_vel(vel))
self.controller.go_forward()
def handleButtonBack(self, vel):
self.controller.set_global_velocity(self.from_real_to_drive_vel(vel))
self.controller.go_back()
def handleButtonR(self, vel):
self.controller.set_global_velocity(self.from_real_to_drive_vel(vel))
self.controller.turn_right()
def handleButtonL(self, vel):
self.controller.set_global_velocity(self.from_real_to_drive_vel(vel))
self.controller.turn_left()
def handleButtonSTP(self):
self.controller.stop_motor()
def test_Controller(self, vel=5000000):
test_object = Controller(self.port)
self.start_info(test_object)
test_object.set_global_velocity(vel/2)
test_object.go_forward()
time.sleep(2)
test_object.set_global_velocity(vel/2)
test_object.go_back()
time.sleep(2)
test_object.set_global_velocity(vel/5)
test_object.turn_right()
time.sleep(5)
test_object.turn_left()
time.sleep(5)
test_object.stop_motor()
class RobotHQ(QtGui.QWidget):
# TODO: 5 buttons, 2 text fields
# GUI (forward/back/right/left/stop, velocity, distance) -> cmd
def __init__(self, port=1, server=None):
self.controller = Controller(port)
self.server = server
self.coef = 58823530
self.app = QtGui.QApplication(sys.argv)
super(RobotHQ, self).__init__()
self.setGeometry(500, 500, 700, 300)
self.setWindowTitle("Mobile Robot control")
self.init_ui()
self.center()
if server:
self.zmq_port = "50060"
self.context = zmq.Context()
print "Connecting to server..."
self.socket = self.context.socket(zmq.REQ)
self.socket.connect("tcp://%s:%s" % (self.server, self.zmq_port))
def init_ui(self):
btn_fwd = QtGui.QPushButton("Move Forward", self)
btn_fwd.clicked.connect(self.handleButtonFWD)
btn_fwd.resize(btn_fwd.sizeHint())
btn_fwd.move(115, 10)
btn_fwd.setFixedWidth(150)
btn_back = QtGui.QPushButton("Back", self)
btn_back.clicked.connect(self.handleButtonBack)
btn_back.resize(btn_back.sizeHint())
btn_back.move(115, 65)
btn_back.setFixedWidth(150)
btn_stop = QtGui.QPushButton("Stop", self)
btn_stop.clicked.connect(self.handleButtonSTP)
btn_stop.resize(btn_stop.sizeHint())
btn_stop.move(115, 95)
btn_stop.setFixedWidth(150)
btn_left = QtGui.QPushButton("Turn Left", self)
btn_left.clicked.connect(self.handleButtonL)
btn_left.resize(btn_stop.sizeHint())
btn_left.move(10, 37)
btn_left.setFixedWidth(150)
btn_right = QtGui.QPushButton("Turn Right", self)
btn_right.clicked.connect(self.handleButtonR)
btn_right.resize(btn_stop.sizeHint())
btn_right.move(210, 37)
btn_right.setFixedWidth(150)
lb_vel = QtGui.QLabel("Velocity [m/s]", self)
lb_vel.move(15, 135)
self.le_vel = QtGui.QLineEdit("0.02", self)
self.le_vel.move(10, 150)
self.le_vel.setFixedWidth(150)
lb_dis = QtGui.QLabel("Position", self)
lb_dis.move(220, 135)
self.le_pos = QtGui.QLineEdit(self)
self.le_pos.move(215, 150)
self.le_pos.setFixedWidth(150)
def from_real_to_drive_vel(self, real):
return float(real) * self.coef
def handleButtonFWD(self):
if self.server:
cmd = []
cmd.append("forward")
cmd.append(str(self.le_vel.text()))
v_send = json.dumps(cmd)
self.socket.send(v_send)
# Get the reply.
message = self.socket.recv()
print message
else:
self.controller.set_global_velocity(self.from_real_to_drive_vel(self.le_vel.text()))
if self.le_pos.text() != "":
# TODO: implement self.controller.set_possition(int(self.le_pos.text()))
pass
self.controller.go_forward()
def handleButtonBack(self):
if self.server:
cmd = []
cmd.append("back")
cmd.append(str(self.le_vel.text()))
v_send = json.dumps(cmd)
self.socket.send(v_send)
# Get the reply.
message = self.socket.recv()
print message
else:
self.controller.set_global_velocity(self.from_real_to_drive_vel(self.le_vel.text()))
self.controller.go_back()
def handleButtonR(self):
if self.server:
cmd = []
cmd.append("right")
cmd.append(str(self.le_vel.text()))
v_send = json.dumps(cmd)
self.socket.send(v_send)
# Get the reply.
message = self.socket.recv()
print message
else:
self.controller.set_global_velocity(self.from_real_to_drive_vel(self.le_vel.text()))
self.controller.turn_right()
def handleButtonL(self):
if self.server:
cmd = []
cmd.append("left")
cmd.append(str(self.le_vel.text()))
v_send = json.dumps(cmd)
self.socket.send(v_send)
# Get the reply.
message = self.socket.recv()
print message
else:
self.controller.set_global_velocity(self.from_real_to_drive_vel(self.le_vel.text()))
self.controller.turn_left()
def handleButtonSTP(self):
if self.server:
cmd = []
cmd.append("stop")
cmd.append(str(self.le_vel.text()))
v_send = json.dumps(cmd)
self.socket.send(v_send)
# Get the reply.
message = self.socket.recv()
print message
else:
self.controller.stop_motor()
def closeEvent(self, event):
reply = QtGui.QMessageBox.question(
self, "Message", "Are you sure to quit?", QtGui.QMessageBox.Yes | QtGui.QMessageBox.No, QtGui.QMessageBox.No
)
if reply == QtGui.QMessageBox.Yes:
event.accept()
else:
event.ignore()
def center(self):
qr = self.frameGeometry()
cp = QtGui.QDesktopWidget().availableGeometry().center()
qr.moveCenter(cp)
self.move(qr.topLeft())
def start_gui(self):
self.show()
sys.exit(self.app.exec_())
