def main():
n = ScreenRecorder((1, 26, 720 + 1, 576 + 26))
controller = KeyboardController()
controller.iniciarJuego()
time.sleep(10)
controller.position_window()
time.sleep(3)
controller.iniciarPartida()
time.sleep(2)
n.capture_live(show=True, save=True, time_frame=60)
controller.finJuego()
def main():
if (len(sys.argv) < 2):
print("Usage: drive.py (port) (baud rate)")
exit(1)
port = sys.argv[1]
baud = int(sys.argv[2])
print("Connecting to {0} @ {1}bps".format(port, baud))
try:
communicator = ChassisCommunicator(port, baud)
except:
print("Could not connect!")
exit(2)
controller = KeyboardController(communicator)
controller.start()
class Server:
audio_controller = AudioController()
communicator = Communicator()
mouse_controller = MouseController()
keyboard_controller = KeyboardController()
def __init__(self):
self.connection = self.init_connection()
self.handle_connection()
@staticmethod
def init_connection():
host = socket.gethostname()
connection = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
connection.bind((host, Constants.PORT_NR))
return connection
def handle_connection(self):
self.start_listen()
try:
while True:
message_str = self.communicator.receive_message()
message = Message.from_string(message_str)
self.read_message(message)
except (ConnectionResetError, ConnectionAbortedError,
TimeoutError) as e:
self.handle_communication_error(e)
def start_listen(self):
self.connection.listen(1)
client_socket, address = self.connection.accept()
self.communicator.set_client_socket(client_socket)
print("waiting for data...")
def handle_communication_error(self, error):
pass
print(str(error))
print('Connection broken, waiting for host reconnection...')
self.handle_connection()
def read_message(self, message):
if not message.command:
self.handle_empty_command()
elif message.command == Constants.COMMAND_PING:
self.handle_ping_command()
elif message.command == Constants.COMMAND_SHUTDOWN_NOW:
self.handle_shutdown_now_command()
elif message.command == Constants.COMMAND_SCHEDULED_SHUTDOWN:
self.handle_scheduled_shutdown_command(message.params)
elif message.command == Constants.COMMAND_ABORT_SHUTDOWN:
self.handle_abort_shutdown_command()
elif message.command == Constants.COMMAND_RESTART:
self.handle_restart_command()
elif message.command == Constants.COMMAND_SET_VOLUME:
self.handle_set_volume_command(message.params)
elif message.command == Constants.COMMAND_GET_VOLUME:
self.handle_get_volume_command()
elif message.command == Constants.COMMAND_MUTE:
self.handle_mute_command()
elif message.command == Constants.COMMAND_UNMUTE:
self.handle_unmute_command()
elif message.command == Constants.COMMAND_MOUSE_MOVE:
self.handle_mouse_move_command(message.params)
elif message.command == Constants.COMMAND_MOUSE_LEFT_CLICK:
self.handle_mouse_left_click_command()
elif message.command == Constants.COMMAND_MOUSE_RIGHT_CLICK:
self.handle_mouse_right_click_command()
elif message.command == Constants.COMMAND_KEYBOARD_FETCH_KEY_STATE:
self.handle_keyboard_fetch_state()
elif message.command == Constants.COMMAND_KEYBOARD_SPECIAL_KEY:
self.handle_keyboard_special_key_command(message.params)
elif message.command == Constants.COMMAND_KEYBOARD_REGULAR_KEY:
self.handle_keyboard_regular_key_command(message.params)
else:
self.handle_unknown_command()
@staticmethod
def handle_empty_command():
print('no data, restarting...')
raise ConnectionAbortedError
def handle_unknown_command(self):
print('unknown command')
self.communicator.send_unsuccessful_response()
def handle_ping_command(self):
print("ping")
self.communicator.send_successful_response([Constants.FEEDBACK_PONG])
def handle_shutdown_now_command(self):
print("shut system down")
self.communicator.send_successful_response()
os.system(WindowsCommands.SHUTDOWN_NOW)
def handle_scheduled_shutdown_command(self, params):
print("shut system down with timeout")
if len(params) == 1:
os.system(WindowsCommands.SHUTDOWN_ABORT)
os.system(WindowsCommands.SCHEDULE_SHUTDOWN + params[0])
self.communicator.send_successful_response()
else:
self.communicator.send_unsuccessful_response()
def handle_abort_shutdown_command(self):
print("system shutdown aborted")
self.communicator.send_successful_response()
os.system(WindowsCommands.SHUTDOWN_ABORT)
def handle_restart_command(self):
print("system restart")
self.communicator.send_successful_response()
os.system(WindowsCommands.RESTART)
def handle_set_volume_command(self, params):
print("set volume")
if len(params) == 1:
level = int(params[0])
self.audio_controller.set_volume(level)
self.communicator.send_successful_response()
else:
self.communicator.send_unsuccessful_response()
def handle_get_volume_command(self):
print("get volume")
current_volume = self.audio_controller.get_volume()
is_muted = self.audio_controller.is_muted()
self.communicator.send_successful_response([current_volume, is_muted])
def handle_mute_command(self):
print("mute")
self.audio_controller.mute()
self.communicator.send_successful_response()
def handle_unmute_command(self):
print("unmute")
self.audio_controller.unmute()
self.communicator.send_successful_response()
def handle_mouse_move_command(self, params):
print("mouse move")
if len(params) == 2:
x_offset = float(params[0])
y_offset = float(params[1])
self.mouse_controller.move(x_offset, y_offset)
self.communicator.send_successful_response()
def handle_mouse_left_click_command(self):
print("mouse left click")
self.mouse_controller.left_click()
self.communicator.send_successful_response()
def handle_mouse_right_click_command(self):
print("mouse right click")
self.mouse_controller.right_click()
self.communicator.send_successful_response()
def handle_keyboard_special_key_command(self, params):
print("keyboard key")
if len(params) == 1:
key = params[0]
self.keyboard_controller.press_special_key(key)
self.communicator.send_successful_response()
def handle_keyboard_regular_key_command(self, params):
print("keyboard key regular")
if len(params) == 1:
key = params[0]
self.keyboard_controller.writeChar(key)
self.communicator.send_successful_response()
def handle_keyboard_fetch_state(self):
print("keyboard fetch state")
state = self.keyboard_controller.get_keys_state()
self.communicator.send_successful_response([state])
args = parser.parse_args()
if args.debug:
logging.basicConfig(level=logging.DEBUG)
else:
logging.basicConfig(level=logging.INFO)
if args.screen:
from screen_matrix import ScreenMatrix
matrix = ScreenMatrix()
else:
try:
from led_matrix import LEDMatrix
except ModuleNotFoundError:
print("Could not load matrix module. either `pip3 install spidev` or use the --screen flag", file=sys.stderr)
sys.exit(1)
matrix = LEDMatrix()
if args.keyboard:
from keyboard_controller import KeyboardController
controller = KeyboardController()
else:
try:
from rotary_controller import RotaryController
except ModuleNotFoundError:
print("Could not load rotary controller module. either `pip3 install evdev` or use the --keyboard flag", file=sys.stderr)
sys.exit(1)
controller = RotaryController()
screen = Screen(8,32, matrix)
t = Tetris(screen, controller)
t.run()
for i, x in enumerate(self.terrain[1::2]):
instruction_group.add(Rectangle(pos=(i, 0), size=(1, x)))
self.instruction = instruction_group
def get_terrain(self) -> list:
terrain = []
step = 1
for x in range(0, Window.width + step, step):
terrain.append(x)
f = pnoise1(x / Window.width * 2,
octaves=6,
base=self.seed,
repeat=2048)
f = f * Window.height / 2 + Window.height * 0.5
terrain.append(int(f))
return terrain
class GameScreen(Screen):
"""
Wrapper for the game widget. It needs to be inside the Screen.
"""
def __init__(self, **kwargs):
super().__init__(**kwargs)
self.game = game
self.add_widget(self.game)
game = ScorchedEarthGame()
kc = KeyboardController()
def __init__(self):
self.state = 0
self.ardrone_state = -1
self.Px = 0.065
self.Ix = 0
self.Dx = 0.07
self.saturation_x = 0.35
self.Py = 0.1
self.Iy = 0
self.Dy = 0.035
self.saturation_y = 0.3
self.Pz = 1.1
self.Iz = 0.01
self.Dz = 0
self.saturation_z = 0.4
self.Pyaw = 0.75
self.Iyaw = 0.065
self.Dyaw = 0.2
self.saturation_yaw = 0.6
self.pidx = PID.PID(self.Px, self.Ix, self.Dx, self.saturation_x)
self.pidy = PID.PID(self.Py, self.Iy, self.Dy, self.saturation_y)
self.pidz = PID.PID(self.Pz, self.Iz, self.Dz, self.saturation_z)
self.pidyaw = PID.PID(self.Pyaw, self.Iyaw, self.Dyaw,
self.saturation_yaw)
self.pidyaw.windup_guard = 1
self.smplTime = 1 / 200
self.height_goal = 0.0
self.horizontal_goal = 0.0
self.distance_goal = 4.5
self.yaw_goal = 0.0
self.pidx.setReference(self.distance_goal)
self.pidy.setReference(self.horizontal_goal)
self.pidz.setReference(self.height_goal)
self.pidyaw.setReference(self.yaw_goal)
self.pidx.setSampleTime(self.smplTime)
self.pidy.setSampleTime(self.smplTime)
self.pidz.setSampleTime(self.smplTime)
self.pidyaw.setSampleTime(self.smplTime)
self.estimated_marker = None
self.marker_seen = False
self.marker_timeout_threshold = 0.5
self.marker_last_seen = 0 - self.marker_timeout_threshold
print('Rescueranger initilizing')
self.pub = rospy.Publisher('object_tracker/pref_pos',
Twist,
queue_size=10)
# Subscribe to the /ardrone/navdata topic, of message type navdata, and
# call self.ReceiveNavdata when a message is received
self.subNavdata = rospy.Subscriber('/ardrone/navdata', Navdata,
self.callback_ardrone_navdata)
# Allow the controller to publish to the /ardrone/takeoff, land and
# reset topics
self.pubLand = rospy.Publisher('/ardrone/land', Empty, queue_size=10)
self.pubTakeoff = rospy.Publisher('/ardrone/takeoff',
Empty,
queue_size=10)
self.pubReset = rospy.Publisher('/ardrone/reset', Empty, queue_size=10)
# Allow the controller to publish to the /cmd_vel topic and thus control
# the drone
self.command = Twist()
self.pubCommand = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
# Publish error relative the marker
self.pub_info_error = rospy.Publisher('/rescueranger/MarkerError',
CoordStruct,
queue_size=10)
self.info_error = CoordStruct()
# Publish error relative the marker
self.pub_info_marker = rospy.Publisher('/rescueranger/MarkerPos',
CoordStruct,
queue_size=10)
self.info_marker = CoordStruct()
# Publish error relative the marker
self.pub_info_vel = rospy.Publisher('/rescueranger/VelocityOutput',
CoordStruct,
queue_size=10)
self.info_vel = CoordStruct()
# Keyboard Controller
self.keyboard = KeyboardController()
self.keyboard.tracker = self
print('...')
rospy.init_node('object_tracker', anonymous=True)
# Marker pose subscriber
rospy.Subscriber('/Estimated_marker', Marker,
self.callback_estimated_marker_pose)
# Marker State
self.estimated_marker_state_map = {
'Waiting': 0, # Not detecting any pattern, just recieving images
'Detected_marker': 1, # Pattern detected
}
print('...initilized\n')
rospy.on_shutdown(self.ardrone_send_land)
def __init__(self):
self.state = 0
self.ardrone_state = -1
print('Rescueranger initilizing')
self.vision_pose = TFMessage()
self.marker_seen = False
self.pub = rospy.Publisher(
'object_tracker/pref_pos',
Twist,
queue_size=10)
# Subscribe to the /ardrone/navdata topic, of message type navdata, and
# call self.ReceiveNavdata when a message is received
self.subNavdata = rospy.Subscriber(
'/ardrone/navdata',
Navdata,
self.callback_ardrone_navdata)
# Allow the controller to publish to the /ardrone/takeoff, land and
# reset topics
self.pubLand = rospy.Publisher('/ardrone/land', Empty)
self.pubTakeoff = rospy.Publisher('/ardrone/takeoff', Empty)
self.pubReset = rospy.Publisher('/ardrone/reset', Empty)
# Allow the controller to publish to the /cmd_vel topic and thus control
# the drone
self.command = Twist()
self.pubCommand = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
# Keyboard Controller
self.keyboard = KeyboardController()
self.keyboard.tracker = self
print('...')
rospy.init_node('object_tracker', anonymous=True)
rospy.Subscriber(
"/tf",
TFMessage,
self.callback_vision_pose)
# VISP STATE
self.visp_state = -1
rospy.Subscriber(
'/visp_auto_tracker/status',
Int8,
self.callback_visp_state)
self.visp_state_map = {
'Waiting': 0, # Not detecting any pattern, just recieving images
'Detect_flash': 1, # Pattern detected
# Model successfully initialized (from wrl & xml files)
'Detect_model': 2,
'Track_model': 3, # Tracking model
# Detecting pattern in a small region around where the pattern was
# last seen
'Redetect_flash': 4,
'Detect_flash2': 5 # Detecting pattern in a the whole frame
}
print('...')
# rospy.Subscriber(
# "/ardrone/predictedPose",
# PoseStamped,
# self.callback_ardrone_prediction)
print('...initilized\n')
rospy.on_shutdown(self.ardrone_send_land)
def __init__(self):
self.status = -1
print('Rescueranger initilizing')
self.visp_pose = PoseStamped()
self.pred_pose = PoseStamped()
self.pub = rospy.Publisher(
'object_tracker/pref_pos',
Twist,
queue_size=10)
# Subscribe to the /ardrone/navdata topic, of message type navdata, and
# call self.ReceiveNavdata when a message is received
self.subNavdata = rospy.Subscriber(
'/ardrone/navdata',
Navdata,
self.callback_ardrone_navdata)
# Allow the controller to publish to the /ardrone/takeoff, land and
# reset topics
self.pubLand = rospy.Publisher('/ardrone/land', Empty)
self.pubTakeoff = rospy.Publisher('/ardrone/takeoff', Empty)
self.pubReset = rospy.Publisher('/ardrone/reset', Empty)
# Allow the controller to publish to the /cmd_vel topic and thus control
# the drone
self.pubCommand = rospy.Publisher('/cmd_vel', Twist)
# Keyboard Controller
self.keyboard = KeyboardController()
print('...')
rospy.init_node('object_tracker', anonymous=True)
rospy.Subscriber(
"/visp_auto_tracker/object_position",
PoseStamped,
self.callback_visp_pose)
# VISP STATE
self.visp_state = -1
rospy.Subscriber(
'/visp_auto_tracker/status',
Int8,
self.callback_visp_state)
self.visp_state_map = {
'Waiting': 0, # Not detecting any pattern, just recieving images
'Detect_flash': 1, # Pattern detected
# Model successfully initialized (from wrl & xml files)
'Detect_model': 2,
'Track_model': 3, # Tracking model
# Detecting pattern in a small region around where the pattern was
# last seen
'Redetect_flash': 4,
'Detect_flash2': 5 # Detecting pattern in a the whole frame
}
print('...')
# rospy.Subscriber(
# "/ardrone/predictedPose",
# PoseStamped,
# self.callback_ardrone_prediction)
print('...initilized\n')
class ObjectTracker(object):
def __init__(self):
self.status = -1
print('Rescueranger initilizing')
self.visp_pose = PoseStamped()
self.pred_pose = PoseStamped()
self.pub = rospy.Publisher(
'object_tracker/pref_pos',
Twist,
queue_size=10)
# Subscribe to the /ardrone/navdata topic, of message type navdata, and
# call self.ReceiveNavdata when a message is received
self.subNavdata = rospy.Subscriber(
'/ardrone/navdata',
Navdata,
self.callback_ardrone_navdata)
# Allow the controller to publish to the /ardrone/takeoff, land and
# reset topics
self.pubLand = rospy.Publisher('/ardrone/land', Empty)
self.pubTakeoff = rospy.Publisher('/ardrone/takeoff', Empty)
self.pubReset = rospy.Publisher('/ardrone/reset', Empty)
# Allow the controller to publish to the /cmd_vel topic and thus control
# the drone
self.pubCommand = rospy.Publisher('/cmd_vel', Twist)
# Keyboard Controller
self.keyboard = KeyboardController()
print('...')
rospy.init_node('object_tracker', anonymous=True)
rospy.Subscriber(
"/visp_auto_tracker/object_position",
PoseStamped,
self.callback_visp_pose)
# VISP STATE
self.visp_state = -1
rospy.Subscriber(
'/visp_auto_tracker/status',
Int8,
self.callback_visp_state)
self.visp_state_map = {
'Waiting': 0, # Not detecting any pattern, just recieving images
'Detect_flash': 1, # Pattern detected
# Model successfully initialized (from wrl & xml files)
'Detect_model': 2,
'Track_model': 3, # Tracking model
# Detecting pattern in a small region around where the pattern was
# last seen
'Redetect_flash': 4,
'Detect_flash2': 5 # Detecting pattern in a the whole frame
}
print('...')
# rospy.Subscriber(
# "/ardrone/predictedPose",
# PoseStamped,
# self.callback_ardrone_prediction)
print('...initilized\n')
def callback_ardrone_navdata(self, navdata):
# Although there is a lot of data in this packet, we're only interested
# in the state at the moment
if self.status != navdata.state:
print("Recieved droneState: %d" % navdata.state)
self.status = navdata.state
def ardrone_send_takeoff(self):
# Send a takeoff message to the ardrone driver
# Note we only send a takeoff message if the drone is landed - an
# unexpected takeoff is not good!
if self.status == DroneStatus.Landed:
self.pubTakeoff.publish(Empty())
def ardrone_send_land(self):
# Send a landing message to the ardrone driver
# Note we send this in all states, landing can do no harm
self.pubLand.publish(Empty())
def ardrone_send_emergency(self):
# Send an emergency (or reset) message to the ardrone driver
self.pubReset.publish(Empty())
def SetCommand(self, roll=0, pitch=0, yaw_velocity=0, z_velocity=0):
# Called by the main program to set the current command
self.command.linear.x = pitch
self.command.linear.y = roll
self.command.linear.z = z_velocity
self.command.angular.z = yaw_velocity
def SendCommand(self, event):
# The previously set command is then sent out periodically if the drone
# is flying
if (
self.status == DroneStatus.Flying or
self.status == DroneStatus.GotoHover or
self.status == DroneStatus.Hovering
):
self.pubCommand.publish(self.command)
def callback_visp_pose(self, pose):
self.visp_pose = pose
def callback_visp_state(self, data):
if data.data != self.visp_state:
self.visp_state = data.data
if data.data == 0:
print("ViSP: Not detecting any pattern, just recieving images")
if data.data == 1:
print("ViSP: Pattern detected")
if data.data == 2:
print(
"ViSP: Model successfully initialized (from wrl & xml files)")
if data.data == 3:
print("ViSP: Tracking model")
if data.data == 4:
print(
"ViSP: Detecting pattern in a small region around where the pattern was last seen")
if data.data == 5:
print("ViSP: Detecting pattern in a the whole frame")
# Predicted pose from tum_ardrone/drone_stateestimation, written to ardrone
def callback_ardrone_prediction(self, pose):
self.pred_pose = pose
def run(self):
print(
'Rescuranger Started\n Publishing to "/object_tracker/pref_pos"\n')
twist = Twist()
# Publish the estimated waypoint on object_tracker/pref_pos
r = rospy.Rate(100) # in Hz
# 0=wait, 1=takeoff, 2=hover over marker, 3= search for marker, 4=
# aproach marker, 5= land, 6= do nothing
state = 0
while not rospy.is_shutdown():
key = self.keyboard.get_key()
if key == self.keyboard.cmd_map['emergency']:
self.ardrone_send_emergency()
if state == 0:
# wait for start command
if True:
state = 1
print("Taking off")
if state == 1: # takeoff
pass
# self.ardrone_send_takeoff()
# if (
# (self.status == DroneStatus.Flying) or
# (self.status == DroneStatus.Hovering)
# ):
# state = 2
if state == 2: # hover over marker
pass
if state == 3: # search for marker
# SetCommand(self,roll=0,pitch=0,yaw_velocity=0,z_velocity=0)
# publish go forward one meter
if True: # vispFoundMarker
state = 5 # change later
print("Landing")
if state == 5:
if self.status == DroneStatus.Landed:
state = 6
if state == 6:
pass # do nothing
twist.linear.x = self.visp_pose.pose.position.x + \
self.pred_pose.pose.position.x
twist.linear.y = self.visp_pose.pose.position.y + \
self.pred_pose.pose.position.y
twist.linear.z = self.visp_pose.pose.position.z + \
self.pred_pose.pose.position.z
self.pub.publish(twist)
r.sleep()
print('\n\nRescuranger is terminating.\n')
self.ardrone_send_emergency()
from keyboard_controller import KeyboardController
import time
controller = KeyboardController()
controller.iniciarJuego()
time.sleep(10)
controller.position_window()
time.sleep(3)
print("COMENZAMOS")
controller.iniciarPartida()
time.sleep(2)
print("SALTAMOS")
controller.accion(1)
time.sleep(5)
print("REPETIMOS")
controller.accion(1)
time.sleep(1)
controller.accion(0)
print("FIN")
controller.finJuego()
