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_())
class ControllerTest:
def __init__(self, node_name):
rospy.init_node(node_name)
# For checking flag positions
rospy.wait_for_service('get_location')
self.get_location_service = rospy.ServiceProxy('get_location', FlagLocation)
rospy.loginfo('Got handle toget_location service.')
# Get controller
self.controller = Controller(rospy)
# Initially go to green flag
# 0 is green
# 1 is red
self.target_color = 0
self.is_centered = False
self.is_close = False
self.tried_recovery = False
self.total_flags = 4
self.completed_flags = 0
rate = rospy.Rate(10)
while not rospy.is_shutdown():
# Look at flag
self.flag_position = self.get_location_service(self.target_color)
# Do I see a flag?
if self.flag_position.x == -1 and self.flag_position.y == -1:
print "Where is the flag?"
if not self.tried_recovery:
self.try_recovery()
rate.sleep()
continue
else:
print "Still couldn't find the flag. I will stop now."
break
# If i got here, I may try recovery again soon.
self.tried_recovery = False
# Center on flag
self.center_on_flag()
# Move to flag
self.approach_flag()
# Turn around flag
if self.is_close:
self.turn_around_flag()
if self.completed_flags == self.total_flags:
self.controller.play_song()
while True:
self.controller.turn_left(0.25, 200)
self.find_next_flag()
# Sweet success:
rate.sleep()
# Center on flag if needed.
def center_on_flag(self):
self.is_centered = False
flag_center = self.flag_position.x + (self.flag_position.w/2.0)
# Some slack in our calculations
X_SLACK = 0.10 * X_MID
if (X_MID - X_SLACK) < flag_center < (X_MID + X_SLACK):
self.is_centered = True
print "Flag is centered!"
elif flag_center > (X_MID + X_SLACK):
self.controller.turn_right(0.1, 25)
self.is_centered = False
print "Gotta turn right..."
else:
self.controller.turn_left(0.1, 25)
self.is_centered = False
print "Gotta turn left..."
# approach if possible
def approach_flag(self):
# only approach if centered...
if self.is_centered:
self.is_close = False
flag_width = self.flag_position.w
# Some slack in our calculations
X_SLACK = 0.0 * X_MID
if flag_width > (X_RES/3) - X_SLACK:
self.is_close = True
print "Pretty close to a flag now."
else:
self.controller.move_forward(0.5, 200)
self.is_close = False
print "Gotta move closer to the flag...width is " + str(flag_width)
def turn_around_flag(self):
print "Time to turn around the flag!"
if self.target_color == 1:
# we go to the right of greens
self.controller.circle_left(6, 200)
else:
# we go to the left of reds.
self.controller.circle_right(6, 200)
# another flag down
self.completed_flags += 1
# finds next flag after turn.
# target i
def find_next_flag(self):
# Swap color, reset values
self.target_color = (self.target_color + 1) % 2
self.is_centered = False
self.is_close = False
print "Where is the next flag?"
while True:
self.flag_position = self.get_location_service(self.target_color)
if self.flag_position.x != -1 and self.flag_position.y != -1:
print "Found it"
break
else:
if self.target_color == 1:
# we should look mostly to the right to find next green flag.
self.controller.turn_left(0.1, 75)
else:
# we should look mostly to the left to find next green flag.
self.controller.turn_right(0.1, 75)
def try_recovery(self):
print "I will now try to recover"
self.controller.move_backwards(1, 100)
if self.flag_visible(self.target_color):
return
# self.controller.turn_left(2, 50)
# if self.flag_visible(self.target_color):
# return
self.controller.move_backwards(1, 100)
if self.flag_visible(self.target_color):
return
# self.controller.turn_right(2, 50)
# if self.flag_visible(self.target_color):
# return
self.controller.move_backwards(1, 100)
if self.flag_visible(self.target_color):
return
# self.controller.turn_right(2, 50)
# if self.flag_visible(self.target_color):
# return
# will stop trying
self.tried_recovery = True
def flag_visible(self, color):
flag_position = self.get_location_service(color)
return flag_position.x != -1 and flag_position.y != -1
