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