def check_key_press(D, key_press):
"""Handles incoming key presses."""
D.last_key_pressed = key_press
if key_press == ord('q') or key_press == 27: # if a 'q' or ESC was pressed
R.move(0, 0)
print "quitting"
rospy.signal_shutdown("Quit requested from keyboard")
elif key_press == ord('h'):
print " Keyboard Command Menu"
print " =============================="
print " ESC/q: quit"
print " h : help menu"
print " s : save thresholds to file"
print " l : load thresholds from file"
print " c : mousedrags will no longer set thresholds, kept values will be cleared"
print " a : mousedrag will assign thresholds to area within drag, \n" + \
" resets on new click or drag"
print " r : mousedrags will remove the area under consideration, \n" + \
" must have set an area in 'a' mode first"
print " m : mousedrags will add the area under consideration, \n" + \
" must have set an area in 'a' mode first"
print " t : show total threshold image in threshold window"
print " A : activate robot for moving, press A again to deactivate "
print " 1 : begin state machine as leader"
print " 2 : begin state machine as follower"
#Save thresholds to file
elif key_press == ord('s'):
fileName = raw_input('Please enter the name of a color: ')
fileName += "_thresholds.txt"
writeFile = open(fileName, "w") #open file for writing
print >> writeFile, D.thresholds
writeFile.close()
#Load thresholds from file
elif key_press == ord('l'):
whichFile = raw_input('Please enter the name of a color: ')
whichFile += "_thresholds.txt"
readFile = open(whichFile, "r") #open file for reading
data = readFile.read()
D.thresholds = eval(data)
readFile.close()
D.loaded_thresholds = True
#Reset threshold sliders
#for thresh in ['red', 'blue', 'green', 'hue', 'sat', 'val']:
# cv.SetTrackbarPos('low_' + thresh, 'Sliders', D.thresholds['low_'+thresh])
# cv.SetTrackbarPos('high_' + thresh, 'Sliders', D.thresholds['high_'+thresh])
# Start picking up thresholded images
elif key_press == ord('a'):
D.mode = "start"
# Clear all loaded sections
elif key_press == ord('c'):
D.mode = "clear"
D.sections = []
# Remove areas from thresholding
elif key_press == ord('r'):
if len(D.sections) > 0:
D.mode = "subtract"
else:
print "Cannot switch modes, need a starting area first. Press 'i' " + \
"to select a starting area."
# Add areas for thresholding
elif key_press == ord('m'):
if len(D.sections) > 0:
D.mode = "add"
else:
print "Cannot switch modes, need a starting area first. Press 'i' " + \
"to select a starting area."
elif key_press == ord('t'):
D.current_threshold = D.threshed_image
# Activate the robot for moving
elif key_press == ord('A'):
StateMachine.activate()
# Activate robot as leader, following a white line
elif key_press == ord('1'):
print "Setting mode to \"leader\"."
StateMachine.state_start("leader")
# Activate robot as follower, following the defined target
elif key_press == ord('2'):
print "Setting mode to \"follower\"."
StateMachine.state_start("follower")
# Client activation, can send and recieve
elif key_press == ord('w'):
serverHost = 'localhost'
serverHost = '192.168.1.101'
#serverHost = '134.173.195.75'
#serverHost = "134.173.195.75"
serverHost = "134.173.43.14"
serverPort = 2012
s = socket.socket(socket.AF_INET,
socket.SOCK_STREAM) #Create a TCP socket
s.connect((serverHost, serverPort)) #Connect to server on the port
# Figure out what to send
message = ""
if True:
s.send("hi") #send the data
data = s.recv(1024) #receive up to 1K bytes
print "I received the message:", data
s.close()
print 'Good bye!'
#Robot keyboard driving controls
elif key_press == 82: #up arrow: drive forward
R.move(250, 250)
elif key_press == 84: #down arrow: drive backward
R.move(-250, -250)
elif key_press == 81: #left arrow: turn left
R.move(-250, 250)
elif key_press == 83: #right arrow: turn right
R.move(250, -250)
elif key_press == 32: #spacebar: stop
R.move(0, 0)
def check_key_press(D, key_press):
"""Handles incoming key presses."""
D.last_key_pressed = key_press
if key_press == ord('q') or key_press == 27: # if a 'q' or ESC was pressed
R.move(0,0)
print "quitting"
rospy.signal_shutdown( "Quit requested from keyboard" )
elif key_press == ord('h'):
print " Keyboard Command Menu"
print " =============================="
print " ESC/q: quit"
print " h : help menu"
print " s : save thresholds to file"
print " l : load thresholds from file"
print " c : mousedrags will no longer set thresholds, kept values will be cleared"
print " a : mousedrag will assign thresholds to area within drag, \n" + \
" resets on new click or drag"
print " r : mousedrags will remove the area under consideration, \n" + \
" must have set an area in 'a' mode first"
print " m : mousedrags will add the area under consideration, \n" + \
" must have set an area in 'a' mode first"
print " t : show total threshold image in threshold window"
print " A : activate robot for moving, press A again to deactivate "
print " 1 : begin state machine as leader"
print " 2 : begin state machine as follower"
#Save thresholds to file
elif key_press == ord('s'):
fileName = raw_input('Please enter the name of a color: ')
fileName += "_thresholds.txt"
writeFile = open(fileName, "w") #open file for writing
print >> writeFile, D.thresholds
writeFile.close()
#Load thresholds from file
elif key_press == ord('l'):
whichFile = raw_input('Please enter the name of a color: ')
whichFile += "_thresholds.txt"
readFile = open(whichFile, "r") #open file for reading
data = readFile.read()
D.thresholds = eval(data)
readFile.close()
D.loaded_thresholds = True
#Reset threshold sliders
#for thresh in ['red', 'blue', 'green', 'hue', 'sat', 'val']:
# cv.SetTrackbarPos('low_' + thresh, 'Sliders', D.thresholds['low_'+thresh])
# cv.SetTrackbarPos('high_' + thresh, 'Sliders', D.thresholds['high_'+thresh])
# Start picking up thresholded images
elif key_press == ord('a'):
D.mode = "start"
# Clear all loaded sections
elif key_press == ord('c'):
D.mode = "clear"
D.sections = []
# Remove areas from thresholding
elif key_press == ord('r'):
if len(D.sections) > 0:
D.mode = "subtract"
else:
print "Cannot switch modes, need a starting area first. Press 'i' " + \
"to select a starting area."
# Add areas for thresholding
elif key_press == ord('m'):
if len(D.sections) > 0:
D.mode = "add"
else:
print "Cannot switch modes, need a starting area first. Press 'i' " + \
"to select a starting area."
elif key_press == ord('t'):
D.current_threshold = D.threshed_image
# Activate the robot for moving
elif key_press == ord('A'):
StateMachine.activate()
# Activate robot as leader, following a white line
elif key_press == ord('1'):
print "Setting mode to \"leader\"."
StateMachine.state_start("leader")
# Activate robot as follower, following the defined target
elif key_press == ord('2'):
print "Setting mode to \"follower\"."
StateMachine.state_start("follower")
# Client activation, can send and recieve
elif key_press == ord ('w'):
serverHost = 'localhost'
serverHost = '192.168.1.101'
#serverHost = '134.173.195.75'
#serverHost = "134.173.195.75"
serverHost = "134.173.43.14"
serverPort = 2012
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) #Create a TCP socket
s.connect((serverHost, serverPort)) #Connect to server on the port
# Figure out what to send
message = ""
if True:
s.send("hi") #send the data
data = s.recv(1024) #receive up to 1K bytes
print "I received the message:", data
s.close()
print 'Good bye!'
#Robot keyboard driving controls
elif key_press == 82: #up arrow: drive forward
R.move(250, 250)
elif key_press == 84: #down arrow: drive backward
R.move(-250, -250)
elif key_press == 81: #left arrow: turn left
R.move(-250, 250)
elif key_press == 83: #right arrow: turn right
R.move(250,-250)
elif key_press == 32: #spacebar: stop
R.move(0,0)
def check_key_press(D, key_press):
"""Handles incoming key presses."""
D.last_key_pressed = key_press
if key_press == ord('q') or key_press == 27: #If a 'q' or ESC was pressed
R.move(0,0)
print "quitting"
rospy.signal_shutdown( "Quit requested from keyboard" )
elif key_press == ord('h'):
print " Keyboard Command Menu"
print " =============================="
print " ESC/q: quit"
print " h : help menu"
print " s : save thresholds to file"
print " l : load thresholds from file"
print " c : mousedrags will no longer set thresholds, kept values will be cleared"
print " a : mousedrag will assign thresholds to area within drag, \n" + \
" resets on new click or drag"
print " r : mousedrags will remove the area under consideration, \n" + \
" must have set an area in 'a' mode first"
print " m : mousedrags will add the area under consideration, \n" + \
" must have set an area in 'a' mode first"
print " t : show total threshold image in threshold window"
print " A : activate robot for moving, press A again to deactivate "
print " 1 : begin state machine as leader"
print " 2 : begin state machine as follower"
#Save thresholds to file
elif key_press == ord('s'):
fileName = raw_input('Please enter the name of a color: ')
fileName += "_thresholds.txt"
writeFile = open(fileName, "w") #open file for writing
print >> writeFile, D.thresholds
writeFile.close()
#Load thresholds from file
elif key_press == ord('l'):
whichFile = raw_input('Please enter the name of a color: ')
whichFile += "_thresholds.txt"
readFile = open(whichFile, "r") #open file for reading
data = readFile.read()
D.thresholds = eval(data)
readFile.close()
D.loaded_thresholds = True
#Reset threshold sliders
#for thresh in ['red', 'blue', 'green', 'hue', 'sat', 'val']:
# cv.SetTrackbarPos('low_' + thresh, 'Sliders', D.thresholds['low_'+thresh])
# cv.SetTrackbarPos('high_' + thresh, 'Sliders', D.thresholds['high_'+thresh])
#Start picking up thresholded images
elif key_press == ord('a'):
D.mode = "start"
#Clear all loaded sections
elif key_press == ord('c'):
D.mode = "clear"
D.sections = []
#Remove areas from thresholding
elif key_press == ord('r'):
if len(D.sections) > 0:
D.mode = "subtract"
else:
print "Cannot switch modes, need a starting area first. Press 'i' " + \
"to select a starting area."
# Add areas for thresholding
elif key_press == ord('m'):
if len(D.sections) > 0:
D.mode = "add"
else:
print "Cannot switch modes, need a starting area first. Press 'i' " + \
"to select a starting area."
#Display thresholded image
elif key_press == ord('t'):
D.current_threshold = D.threshed_image
# Activate the robot for moving
elif key_press == ord('A'):
StateMachine.activate()
# Activate robot as leader, following a white line
elif key_press == ord('1'):
print "Setting mode to \"leader\"."
R.curState = "state_lead"
StateMachine.state_start("leader")
# Activate robot as follower, following the defined target
elif key_press == ord('2'):
print "Setting mode to \"follower\"."
R.curState = "state_follow"
StateMachine.state_start("follower")
#Robot keyboard driving controls
elif key_press == 82: #Up arrow: go forward
R.move(80, 80)
elif key_press == 84: #Down arrow: go backwards
R.move(-50, -50)
elif key_press == 81: #Left arrow: turn left
R.move(-80, 80)
elif key_press == 83: #Right arrow: turn right
R.move(80,-80)
elif key_press == 32: #Spacebar: stop
R.move(0,0)
