Finally added takeoff/land keyboard commands

- Moved panda3d camera to its own folder
- Made drone movement faster
- Removed my experimental AI stuff, will be added back to a different repo

Author: nb-programmer

dronesim/__main__.py

@@ -3,6 +3,9 @@
 from dronesim import DroneSimulator
 from dronesim.interface import DefaultDroneControl
 
+#Sensors
+from dronesim.sensor.panda3d.camera import Panda3DCameraSensor
+
 #App window
 from dronesim.simapp import SimulatorApplication
 
@@ -16,8 +19,19 @@ def main():
     #Simulator environment
     drone = DroneSimulator()
     droneControl = DefaultDroneControl(drone, update_enable=True)
+    #Application
     droneWindow = SimulatorApplication(droneControl)
+    #== Add a camera sensor facing down ==
+    downCam = Panda3DCameraSensor("downCameraRGB", size=(320,320))
+    drone.addSensor(down_camera_rgb = downCam)
+    #Insert camera into main app window and attach to the scene
+    downCam.attachToEnv(droneWindow.render, droneWindow.graphicsEngine, droneWindow.win)
+    #Move and rotate camera with the UAV object
+    downCam.reparentTo(droneWindow.getUAVModelNode())
+    #Face down
+    downCam.setHpr(0, -90, 0)
+    #Start the app
     droneWindow.run()
-    
+
 if __name__ == "__main__":
     main()

dronesim/interface/control.py

@@ -1,6 +1,6 @@
 from abc import ABC
 
-from ..types import StepRC
+from ..types import StepRC, DroneAction
 
 class IDroneControllable(ABC):
     '''
@@ -31,3 +31,5 @@ def rotate_counterclockwise(self, x : float):
         pass
     def freeze(self):
         pass
+    def directAction(self, action : DroneAction, args : dict = None):
+        pass

dronesim/interface/default.py

@@ -89,3 +89,10 @@ def freeze(self):
         self.__cmd_queue.put_nowait({
             'action': DroneAction.STOPINPLACE
         })
+
+    def directAction(self, action : DroneAction, args : dict = None):
+        if args is None: args = {}
+        self.__cmd_queue.put_nowait({
+            'action': action,
+            **args
+        })

dronesim/physics/simple.py

@@ -15,7 +15,7 @@ class SimpleDronePhysics(DronePhysicsEngine):
     GRAVITY = glm.vec3(0, 0, -1e-3)
     AIR_RESISTANCE = glm.vec3((0.95,)*3)
     #Angle is counter-clockwise from 3 o'clock position, so negate the magnitude to turn properly
-    RC_SCALE = glm.vec4(0.09, 0.09, 0.05, -0.018)
+    RC_SCALE = glm.vec4(0.09, 0.09, 0.09, -0.05)
 
     #PID controller parameters
     STRAFE_CONTROL_PARAM = {'Kp': 0.45, 'Ki': 0.01, 'Kd': 0.0}

dronesim/sensor/camera.py renamed to dronesim/sensor/panda3d/camera.py

@@ -1,5 +1,5 @@
 
-from .sensor import SensorBase
+from ..sensor import SensorBase
 from panda3d.core import (
     NodePath,
     Camera,
@@ -16,7 +16,7 @@ class Panda3DCameraSensor(NodePath, SensorBase):
     CAMERA_TYPE_RGB = GraphicsOutput.RTPColor
     CAMERA_TYPE_DEPTH = GraphicsOutput.RTPDepth
 
-    def __init__(self, node_name : str, size = (512, 512), camera_type = CAMERA_TYPE_RGB):
+    def __init__(self, node_name : str, size : tuple = (512, 512), camera_type = CAMERA_TYPE_RGB):
         super().__init__(Camera(node_name, PerspectiveLens()))
         self.tex = Texture()
         self.texfbuf = None
@@ -25,6 +25,8 @@ def __init__(self, node_name : str, size = (512, 512), camera_type = CAMERA_TYPE
     def update(self):
         '''Force render scene and return the image as a 'BGR' numpy array'''
         return
+    def getViewportSize(self) -> tuple:
+        return self.fbufsize
     def attachToEnv(self, scene : NodePath, gengine : GraphicsEngine, host_go : GraphicsOutput):
         #TODO: Check if this is even required
         self.node().setScene(scene)

dronesim/simapp.py

@@ -21,7 +21,7 @@
 from . import PACKAGE_BASE
 from .interface.control import IDroneControllable
 from .utils import IterEnumMixin, HUDMixin, rad2deg
-from .types import Vec4Tuple
+from .types import Vec4Tuple, DroneAction
 
 import os
 import glm
@@ -358,6 +358,8 @@ def __init__(self,
         self.accept("f5", self.eToggleCameraMode)
         self.accept("f6", self.eToggleControlMode)
         self.accept("f11", self.eToggleFullscreen)
+        self.accept("i", self.eHandleDroneCommandSend, [DroneAction.TAKEOFF])
+        self.accept("k", self.eHandleDroneCommandSend, [DroneAction.LAND])
 
         #Tasks
         self.updateEngineTask = self.taskMgr.add(self.updateEngine, "updateEngine")
@@ -464,6 +466,9 @@ def _updatePlayerMovementCommand(self):
         if self.camState.control == ControllingCharacter.player and self.movementState is not None:
             self.drone.rc_control(self.movementState)
 
+    def eHandleDroneCommandSend(self, cmd : DroneAction, params : dict = None):
+        self.drone.directAction(cmd, params)
+
     def _getMovementControlState(self) -> Vec4Tuple:
         #Returns whether a button/key is pressed
         isDown : typing.Callable[[ButtonHandle], bool] = self.mouseWatcherNode.isButtonDown
@@ -473,13 +478,21 @@ def _getMovementControlState(self) -> Vec4Tuple:
         btnL = KeyboardButton.ascii_key('a')
         btnR = KeyboardButton.ascii_key('d')
 
+        btnLArrow = KeyboardButton.left()
+        btnRArrow = KeyboardButton.right()
+        btnUArrow = KeyboardButton.up()
+        btnDArrow = KeyboardButton.down()
+
         state_lr = (isDown(btnR) - isDown(btnL))
         state_fwbw = (isDown(btnFw) - isDown(btnBw))
+        state_yawlr = (isDown(btnRArrow) - isDown(btnLArrow))
+        state_altud = (isDown(btnUArrow) - isDown(btnDArrow))
 
         #Order is the same as the arguments of 'StepRC' type
-        return (state_lr, state_fwbw, 0.0, 0.0)
+        return (state_lr, state_fwbw, state_altud, state_yawlr)
 
     def _updateHUD(self):
+        #TODO: Any other way to hide OnscreenText?
         if not self.HUDState['visible']:
             self.camHUDText.setText('')
             self.debugHUDText.setText('')

examples/follow.py

@@ -3,68 +3,19 @@
 from dronesim.simapp import SimulatorApplication
 from dronesim.interface import IDroneControllable
 from dronesim.types import DroneAction, StepRC
-from dronesim.sensor.camera import Panda3DCameraSensor
+from dronesim.sensor.panda3d.camera import Panda3DCameraSensor
 
 import threading
 import time
 from collections import namedtuple
 
-#AI stuff
-import torch
-import torch.nn as nn
-import torch.optim as optim
-import torch.nn.functional as F
-import torchvision.transforms as T
-
-TENSOR_DEVICE = torch.device("cuda" if torch.cuda.is_available() else "cpu")
-Transition = namedtuple('Transition', ('state', 'action', 'next_state', 'reward'))
-class ReplayMemory(object):
-    def __init__(self, capacity):
-        self.memory = deque([],maxlen=capacity)
-
-    def push(self, *args):
-        """Save a transition"""
-        self.memory.append(Transition(*args))
-
-    def sample(self, batch_size):
-        return random.sample(self.memory, batch_size)
-
-    def __len__(self):
-        return len(self.memory)
-    
-
-'''
-I/O Requirements:
-Inputs:
- - n-vector (n=number of distance rays). Value is intersection distance with path, or 0 if none
- - scalar, for use in decisions or additional input
-            (junction decision for example, -1=left, 1=right, 0=stop or u-turn)
-Outputs:
- - vec3 - 2-degree of movements (l/r, fw/bw) and yaw control,
-            representing the q-values of each action
-
-'''
-
-class DQN(nn.Module):
-    def __init__(self, input_distances : int):
-        super().__init__()
-        self.dense1 = nn.Linear(input_distances, 16)
-        self.dense2 = nn.Linear(16, 32)
-        self.output = nn.Linear(32, 3)
-        
-    def forward(self, x):
-        x = x.to(TENSOR_DEVICE)
-        x = self.dense1(x)
-        x = self.dense2(x)
-        return self.output(x)
-
 class AIDroneControlAgent(threading.Thread, IDroneControllable):
     def __init__(self):
         super().__init__(daemon=True)
 
         #Environment
         self.drone = DroneSimulator()
-        self.model = DQN(10)
+        self.model = None
 
         #Add some sensors
         self.drone.addSensor(
@@ -73,7 +24,6 @@ def __init__(self):
         )
 
         self.__state = dict()
-        self.start()
 
     def initEnv(self, app):
         self.drone.sensors['down_camera_rgb'].attachToEnv(app.render, app.graphicsEngine, app.win)
@@ -88,17 +38,14 @@ def initEnv(self, app):
     def run(self):
         self.drone.state = {"pos": 123}
         self.drone.step({'action': DroneAction.TAKEOFF})
+        print(self.drone.sensors['down_camera_rgb'].getViewportSize())
         for _ in range(200):
             state = self.drone.step()
         for _ in range(2000):
             state = self.drone.step(StepRC(0,0.6,0,0))
             self.__update_debug_state(state)
             time.sleep(0.01)
 
-        #observation, reward, done, info = state
-        #output = self.model.forward(torch.ones(1, 10))
-        #print(output)
-    
     def __update_debug_state(self, state):
         observation, reward, done, info = state
         self.__state.update({
@@ -120,6 +67,7 @@ def main():
     droneControl = AIDroneControlAgent()
     droneWindow = SimulatorApplication(droneControl)
     droneControl.initEnv(droneWindow)
+    droneControl.start()
     droneWindow.run()
 
 if __name__ == "__main__":