def line_model(self, r, g, b):
line = LineNodePath(self.render2d, 'box', 2)
line.reparentTo(self.render)
line.setColor(r, g, b, 1.)
line.setTransparency(TransparencyAttrib.MAlpha)
line.setAlphaScale(0.5)
return line
class World(ShowBase):
fps_text = fps_text2 = fps_text3 = fps_text4 = None
room_dimentions = [0, 0]
camera_position = [290., 1609., 370, -1980, -15, 0] # x y z h p r
drone = None
drone_instance = None
markers = {}
marker_lines = {}
marker_lines_observed = {}
active_keys = {}
loop_callback = None
joy = None
simulation = True
drone_started = False
def __init__(self, width=6.78, length=5.82, simulation=True, video=True):
ShowBase.__init__(self)
width *= 100
length *= 100
self.room_dimentions = [width, length]
self.simulation = simulation
self.wall1 = self.wall_model(0, 0, 0, width, 0)
self.wall2 = self.wall_model(0, 0, 0, length, 90)
self.wall3 = self.wall_model(width, length, 0, width, 180)
self.wall4 = self.wall_model(width, length, 0, length, -90)
self.root_3d = self.marker_model(position=[0., 0., 0.],
orientation=[90, 90, 0])
self.drone = self.drone_model()
self.drone_instance = Drone(simulation=simulation, video=video)
self.lx_drone = LineNodePath(self.render2d, 'box', 2)
self.lx_drone.reparentTo(self.drone)
self.lx_drone.setColor(1., 0., 0., 1.)
self.lx_drone.setTransparency(TransparencyAttrib.MAlpha)
self.lx_drone.setAlphaScale(0.5)
self.lx_drone.drawLines([[(0., 0., 0.), (4., 0., 0.)]])
self.lx_drone.create()
self.ly_drone = LineNodePath(self.render2d, 'box', 2)
self.ly_drone.reparentTo(self.drone)
self.ly_drone.setColor(0., 1., 0., 1.)
self.ly_drone.setTransparency(TransparencyAttrib.MAlpha)
self.ly_drone.setAlphaScale(0.5)
self.ly_drone.drawLines([[(0., 0., 0.), (0., 0., 4.)]])
self.ly_drone.create()
self.lz_drone = LineNodePath(self.render2d, 'box', 2)
self.lz_drone.reparentTo(self.drone)
self.lz_drone.setColor(0., 0., 1., 1.)
self.lz_drone.setTransparency(TransparencyAttrib.MAlpha)
self.lz_drone.setAlphaScale(0.5)
self.lz_drone.drawLines([[(0., 0., 0.), (0., 4., 0.)]])
self.lz_drone.create()
try:
self.joy = xbox.Joystick()
joy_ready = False
if not self.joy.A():
joy_ready = True
if not joy_ready:
raise Exception("Joy not ready!")
else:
print("ready")
except:
pass
# Add the spinCameraTask procedure to the task manager.
self.tick_loop = self.taskMgr.add(self.tick, "tick_loop")
self.accept("space", self.control_drone, [" "])
self.accept("c", self.control_drone, ["c"])
self.accept("x", self.control_drone, ["x"])
self.accept("w", self.control_drone, ["w"])
self.accept("a", self.control_drone, ["a"])
self.accept("s", self.control_drone, ["s"])
self.accept("d", self.control_drone, ["d"])
self.accept("q", self.control_drone, ["q"])
self.accept("e", self.control_drone, ["e"])
self.accept("m", self.control_drone, ["m"])
self.accept("r", self.control_drone, ["r"])
self.accept("f", self.control_drone, ["f"])
self.keypress_repeat("4", self.move_camera, ["x", -1])
self.keypress_repeat("6", self.move_camera, ["x", 1])
self.keypress_repeat("8", self.move_camera, ["y", 1])
self.keypress_repeat("5", self.move_camera, ["y", -1])
self.keypress_repeat("1", self.move_camera, ["z", 1])
self.keypress_repeat("3", self.move_camera, ["z", -1])
self.keypress_repeat("7", self.move_camera, ["h", -1])
self.keypress_repeat("9", self.move_camera, ["h", 1])
self.keypress_repeat("arrow_up", self.move_camera, ["p", 1])
self.keypress_repeat("arrow_down", self.move_camera, ["p", -1])
def control_drone(self, key):
if key == " ":
if not self.drone_started:
self.drone_started = True
self.drone_instance.takeoff()
else:
self.drone_started = False
self.drone_instance.land()
if key == "x":
self.drone_instance.emergency()
elif key == "c":
self.drone_instance.move(0., 0., 0., 0.)
elif key == "w":
self.drone_instance.move(0., 0.08, 0., 0.)
elif key == "s":
self.drone_instance.move(0., -0.08, 0., 0.)
elif key == "d":
self.drone_instance.move(0.08, 0., 0., 0.)
elif key == "a":
self.drone_instance.move(-0.08, 0., 0., 0.)
elif key == "q":
self.drone_instance.move(0., 0., 0., 0.3)
elif key == "e":
self.drone_instance.move(0., 0., 0., -0.3)
elif key == "m":
self.drone_instance.mtrim()
elif key == "r":
self.drone_instance.move(0., 0., 0.14, 0.)
elif key == "f":
self.drone_instance.move(0., 0., -0.15, 0.)
def keypress_repeat(self, key, callback, parameter):
self.accept(key, self.keypress_start, [key, callback, parameter])
self.accept(key + "-up", self.keypress_stop, [key])
def keypress_start(self, key, callback, parameter):
self.active_keys[key] = [callback, parameter]
def keypress_stop(self, key):
self.active_keys[key] = None
def move_camera(self, parameter):
if parameter[0] == "x":
self.camera_position[0] += 10 * np.cos(
np.deg2rad(self.camera_position[3])) * parameter[1]
self.camera_position[1] += -10 * np.sin(
np.deg2rad(self.camera_position[3])) * parameter[1]
if parameter[0] == "y":
self.camera_position[0] += 10 * np.sin(
np.deg2rad(self.camera_position[3])) * parameter[1]
self.camera_position[1] += 10 * np.cos(
np.deg2rad(self.camera_position[3])) * parameter[1]
if parameter[0] == "z":
self.camera_position[2] += 10 * parameter[1]
if parameter[0] == "h":
self.camera_position[3] += parameter[1]
if parameter[0] == "p":
self.camera_position[4] += parameter[1]
def joy_block(self, xbox_key):
""" blocks the xbox key until it's released """
while xbox_key():
pass
def tick(self, task):
for key in self.active_keys:
if self.active_keys[key] is not None:
self.active_keys[key][0](self.active_keys[key][1])
if self.joy is not None:
if self.joy.Back():
self.closeWindow(self.win)
self.userExit()
self.shutdown()
self.destroy()
# takeoff:
if self.joy.A():
print "takeoff"
self.drone_instance.takeoff()
self.joy_block(self.joy.A)
# emergency:
if self.joy.X():
print "emergency"
self.drone_instance.emergency()
self.joy_block(self.joy.X)
# emergency:
if self.joy.B():
print "land"
self.drone_instance.land()
self.joy_block(self.joy.B)
(roll, throttle) = self.joy.leftStick()
(yaw, pitch) = self.joy.rightStick()
print roll, pitch, throttle, yaw
self.drone_instance.move(roll, pitch, throttle, yaw)
if self.loop_callback is not None:
self.loop_callback(self, task)
self.camera.setPos(self.camera_position[0], self.camera_position[1],
self.camera_position[2])
self.camera.setHpr(-self.camera_position[3], self.camera_position[4],
self.camera_position[5])
drone_position = self.convert_position(
self.drone_instance.get_position())
drone_orientation = self.drone_instance.get_orientation()
self.drone.setPos(drone_position[0], drone_position[1],
drone_position[2])
self.drone.setHpr(drone_orientation[0], drone_orientation[1],
drone_orientation[2])
if task.time > 0:
if self.fps_text is not None:
self.fps_text.destroy()
self.fps_text = OnscreenText(text="Tick-Rate: " +
str(int(task.frame / task.time)),
pos=(0.05, 0.05),
scale=0.05,
align=TextNode.ALeft,
parent=self.a2dBottomLeft)
if self.fps_text2 is not None:
self.fps_text2.destroy()
self.fps_text2 = OnscreenText(text="Camera Position: " +
str(self.camera_position),
pos=(0.05, 0.1),
scale=0.05,
align=TextNode.ALeft,
parent=self.a2dBottomLeft)
if self.fps_text3 is not None:
self.fps_text3.destroy()
self.fps_text3 = OnscreenText(
text="Drone Position: " +
str(self.drone_instance.get_position()),
pos=(0.05, 0.15),
scale=0.05,
align=TextNode.ALeft,
parent=self.a2dBottomLeft)
return Task.cont
def drone_model(self):
drone = self.loader.loadModel("resources/models_3d/drone")
drone.setScale(15, 15, 10)
drone.reparentTo(self.render)
return drone
def wall_model(self, x, y, z, length, orientation):
wall = self.loader.loadModel("resources/models_3d/plane")
wall.reparentTo(self.render)
wall.setScale(length, 0, 290)
wall.setPos(x, y, z)
wall.setH(orientation)
wall.setTransparency(TransparencyAttrib.MAlpha)
wall.setAlphaScale(0.1)
return wall
def marker_model(self, position, orientation):
marker = self.loader.loadModel("resources/models_3d/plane")
marker.reparentTo(self.render)
marker.setScale(21, 0, 21)
marker.setPos(position[0], position[1], position[2])
marker.setHpr(orientation[0], orientation[1], orientation[2])
marker.setTransparency(TransparencyAttrib.MAlpha)
marker.setAlphaScale(0.5)
marker.setColor(1., 0., 0., 1.)
return marker
def line_model(self, r, g, b):
line = LineNodePath(self.render2d, 'box', 2)
line.reparentTo(self.render)
line.setColor(r, g, b, 1.)
line.setTransparency(TransparencyAttrib.MAlpha)
line.setAlphaScale(0.5)
return line
def point_model(self, position, color):
marker = self.loader.loadModel("resources/models_3d/sphere")
marker.reparentTo(self.render)
marker.setScale(5, 5, 5)
marker.setPos(position[0], position[1], position[2])
marker.setTransparency(TransparencyAttrib.MAlpha)
marker.setAlphaScale(0.5)
marker.setColor(color[0], color[1], color[2], 1.)
def line_draw(self, line, from_position, to_position):
line.reset()
line.drawLines([[(from_position[0], from_position[1],
from_position[2]),
(to_position[0], to_position[1], to_position[2])]])
line.create()
def get_dimensions(self):
return [self.room_dimentions[0] / 100., self.room_dimentions[1] / 100.]
def get_drone(self):
return self.drone_instance
def set_markers(self, markers):
self.markers = {}
for key, marker in markers.iteritems():
self.markers[key] = self.marker_model(
self.convert_position(marker[0]), marker[1])
def set_default_markers(self):
dimensions = self.get_dimensions()
self.set_markers({
0: [[dimensions[0] / 2, 1.5, 0.01], [0, 0, 0]],
1: [[dimensions[0] / 2, 1.5, dimensions[1] - 0.01], [0, 0, 0]],
2: [[dimensions[0] - 0.01, 1.5, dimensions[1] / 2], [90, 0, 0]],
3: [[0.01, 1.5, dimensions[1] / 2], [90, 0, 0]]
})
def get_markers(self):
return self.markers
def convert_position(self, position):
return [position[0] * 100, position[2] * 100, position[1] * 100]
def hook_init(self, callback):
callback(self)
def hook_loop(self, callback):
self.loop_callback = callback
