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")