def run(self): try: sendTime = 0 while True: #**not rospy.is_shutdown(): self.socket.setblocking(1) #send last movement data to the client if time has passed if sendTime < time.time(): #**delete sendData(self.socket, self.distributor.data) sendTime = time.time() + 1 / float( sendRate) #**sendRate.sleep() try: self.socket.setblocking(0) #get new command newCommand = receiveData(self.socket) #add command to execution queue if newCommand.eStop: commandQueue.insert(0, newCommand) else: commandQueue.append(newCommand) except socket.error: continue except socket.error: #lost connection, stop robot newCommand = MovementData() newCommand.eStop = True commandQueue.insert(0, newCommand) return return
def sendCommand(self): #send new command data sendData(self.sock, self.currentData) time.sleep(2) if self.currentData.endProgram: self.sock.shutdown(socket.SHUT_RDWR) self.sock.close() print("command robot is closed")
def press(key, start): change = True if start == "": data = ManualData() else: data = start if key == Key.up: data.manualDrive = 1 print("Set command to drive forward") elif key == KeyCode.from_char('2'): data.manualDrive = 2 print("Set command to drive forward slowly") elif key == Key.down: data.manualDrive = -1 print("Set command to drive backwards") elif key == Key.right: data.manualTurn = 1 sendData(sock, data) print("Send command to turn right ***") elif key == Key.left: data.manualTurn = -1 sendData(sock, data) print("Send command to turn left ***") elif key == KeyCode.from_char('r'): data.raiseForDig = 1 print("Set command to raise digger") elif key == KeyCode.from_char('l'): data.raiseForDig = -1 print("Set command to lower digger") elif key == KeyCode.from_char('c'): speed = int(input("Dig speed: ")) data.dig = speed print("Set command to dig (collect)") elif key == KeyCode.from_char('p'): data.packin = True sendData(sock, data) print("Send command to pack in ***") elif key == KeyCode.from_char('d'): speed = int(input("Dump speed: ")) data.dump = speed print("Set command to dump") elif key == Key.space: data.stop = True sendData(sock, data) print("Send command to stop ***") elif key == KeyCode.from_char('s'): print("-----Send command-----") sendData(sock, data) else: change = False print("Not a valid command") if change: return data else: return ""
def on_release(key): if key == Key.esc: data = MovementData() sendData(sock, data) time.sleep(2) sock.shutdown(socket.SHUT_RDWR) sock.close() s.shutdown(socket.SHUT_RDWR) s.close() return False
def on_release(key): if key == Key.esc: d = ManualData() d.endProgram = True sendData(sock, d) time.sleep(2) sock.shutdown(socket.SHUT_RDWR) sock.close() s.shutdown(socket.SHUT_RDWR) s.close() return False
def createConnection(self): with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s: #connect to host s.connect((self.HOST,self.PORT)) while True: #receive last data send receiveData(s) #send new data sendData(s, self.currentData)
def run(self): try: sendTime = 0 while True: self.socket.setblocking(1) if sendTime < time.time(): # Send the robot data to the client sendData(self.socket, self.distributor.data) sendTime = time.time() + 1/float(sendRate) print sendTime # An extra exception because we have a non-blocking socket try: self.socket.setblocking(0) # Receive a command and add it to the command queue newCommand = receiveData(self.socket) if newCommand.e_stop: commandQueue.insert(0, newCommand) else: commandQueue.append(newCommand) print "received command" # print manualControlCommand.go_forward except socket.error: continue except socket.error as e: print "Lost connection with " + self.address[0] # Add a E-STOP command newCommand = ManualControlData() newCommand.e_stop = True commandQueue.insert(0, newCommand) return return
def on_press(key): data = MovementData() if key == Key.space: data.data = 7 sendData(sock, data) time.sleep(.1) print("STOP") sendData(sock, data) elif key == KeyCode.from_char('q'): print("exit") data.data = 7 sendData(sock, data) sock.shutdown(socket.SHUT_RDWR) sock.close() s.shutdown(socket.SHUT_RDWR) s.close() exit() elif key == KeyCode.from_char('i'): data.data = 8 print("pack in") sendData(sock, data) elif key == KeyCode.from_char('o'): data.data = 9 print("pack out") sendData(sock, data) elif key == Key.up: data.data = 1 print("drive forward") sendData(sock, data) elif key == Key.down: data.data = 2 print("drive backward") sendData(sock, data) elif key == Key.left: data.data = 3 print("drive left and forward") sendData(sock, data) elif key == Key.right: data.data = 4 print("drive right and forward") sendData(sock, data) elif key == KeyCode.from_char('a'): data.data = 5 print("articulate left") sendData(sock, data) elif key == KeyCode.from_char('d'): data.data = 6 print("articulate right") sendData(sock, data) elif key == KeyCode.from_char('r'): data.data = 11 print("raise bucket chain") sendData(sock, data) elif key == KeyCode.from_char('l'): data.data = 12 print("lower bucket chain") sendData(sock, data) elif key == KeyCode.from_char('t'): data.data = 10 print("turn bucket chain") sendData(sock, data) elif key == KeyCode.from_char('c'): data.data = 15 print("turn conveyor belt") sendData(sock, data) elif key == KeyCode.from_char('f'): data.data = 13 print("raise conveyor belt") sendData(sock, data) elif key == KeyCode.from_char('v'): data.data = 14 print("lower conveyor belt") sendData(sock, data) elif key == KeyCode.from_char('9'): data.data = 999 print("test function ") sendData(sock, data) else: print("Not a valid command")
def on_press(key): data = ManualData() if key == Key.up: data.manualDrive = 1 print("Set command to drive forward") sendData(sock, data) elif key == Key.down: data.manualDrive = -1 print("Set command to drive backwards") sendData(sock, data) elif key == KeyCode.from_char('1'): data.drive = 100 print("Set command to drive backwards") sendData(sock, data) elif key == KeyCode.from_char('2'): data.drive = -100 print("Set command to drive backwards") sendData(sock, data) elif key == Key.right: data.manualTurn = 1 print("Set command to turn right") sendData(sock, data) elif key == Key.left: data.manualTurn = -1 print("Set command to turn left") sendData(sock, data) elif key == KeyCode.from_char('e'): data.dig = True print("Set command to excavate") sendData(sock, data) elif key == KeyCode.from_char('p'): data.packin = True print("Set command to pack in") sendData(sock, data) elif key == KeyCode.from_char('d'): data.dump = True print("Set command to dump") sendData(sock, data) elif key == Key.space: data.stop = True print("Set command to stop") sendData(sock, data) elif key == KeyCode.from_char('f'): data.forwardScan = True print("Scan LiDAR forward") sendData(sock, data) elif key == KeyCode.from_char('b'): data.backwardScan = True print("Scan LiDAR backwards") sendData(sock, data) elif key == KeyCode.from_char('a'): data.autonomousMode = True print("switch from manual to autonomous") sendData(sock, data) else: print("Not a valid command")
def on_press(key): data = ManualData() if key == Key.up: data.manualDrive = 1 print("Set command to drive forward") sendData(sock, data) elif key == KeyCode.from_char('s'): data.manualDrive = 2 print("Set command to drive slowly") sendData(sock, data) elif key == KeyCode.from_char('0'): data.raiseForDig = -2 sendData(sock, data) time.sleep(.1) data.raiseForDig = 0 print("Set command to lower and dig slowly") sendData(sock, data) elif key == KeyCode.from_char('9'): data.raiseForDig = -3 sendData(sock, data) time.sleep(.1) data.raiseForDig = 0 print("Set command to lower and dig extra slowly") sendData(sock, data) elif key == KeyCode.from_char('1'): data.manualDrive = 2 speed = int(input("Dig speed: ")) data.dig = speed print("Set command to lower and dig") sendData(sock, data) elif key == KeyCode.from_char('2'): data.raiseForDig = -1 speed = int(input("Dig speed: ")) data.dig = speed data.manualDrive = 2 print("Set command to lower, dig, drive forward slowly") sendData(sock, data) elif key == KeyCode.from_char('3'): data.raiseForDig = -1 data.manualDrive = 1 print("Set command to lower and drive normal") sendData(sock, data) elif key == KeyCode.from_char('4'): data.manualDrive = 3 print("Set command to unstick from left side") sendData(sock, data) elif key == KeyCode.from_char('5'): data.manualDrive = 4 print("Set command to unstick from right side") sendData(sock, data) elif key == KeyCode.from_char('6'): data.manualDrive = 5 print("Set command to unstick from back") sendData(sock, data) elif key == KeyCode.from_char('7'): data.manualDrive = 6 print("Set command to unstick from front") sendData(sock, data) elif key == Key.down: data.manualDrive = -1 print("Set command to drive backwards") sendData(sock, data) elif key == Key.right: data.manualTurn = 1 print("Set command to turn right") sendData(sock, data) elif key == Key.left: data.manualTurn = -1 print("Set command to turn left") sendData(sock, data) elif key == KeyCode.from_char('r'): data.raiseForDig = 1 print("Set command to raise digger") sendData(sock, data) elif key == KeyCode.from_char('l'): data.raiseForDig = -1 print("Set command to lower digger") sendData(sock, data) elif key == KeyCode.from_char('c'): speed = int(input("Dig speed: ")) data.dig = speed print("Set command to dig (collect)") sendData(sock, data) elif key == KeyCode.from_char('p'): data.packin = True print("Set command to pack in") sendData(sock, data) elif key == KeyCode.from_char('d'): speed = int(input("Dump speed: ")) data.dump = speed print("Set command to dump") sendData(sock, data) elif key == Key.space: data.stop = True print("Set command to stop") sendData(sock, data) else: print("Not a valid command")