Manual car

simpylc/simulations/car_manual_flex_track/control_client/manual_client.py

@@ -0,0 +1,123 @@
+'''
+====== Legal notices
+
+Copyright (C) 2013 - 2021 GEATEC engineering
+
+This program is free software.
+You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense.
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY, without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+See the QQuickLicense for details.
+
+The QQuickLicense can be accessed at: http://www.qquick.org/license.html
+
+__________________________________________________________________________
+
+
+ THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !!
+
+__________________________________________________________________________
+
+It is meant for training purposes only.
+
+Removing this header ends your license.
+'''
+
+import time as tm
+import traceback as tb
+import math as mt
+import sys as ss
+import os
+import socket as sc
+import curses as cs
+
+ss.path +=  [os.path.abspath (relPath) for relPath in  ('..',)] 
+
+import socket_wrapper as sw
+import parameters as pm
+
+class ManualClient:
+    def __init__ (self):
+        self.steeringAngle = 0
+        self.targetVelocity = 0
+
+        self.io = cs.initscr () # take over terminal
+        cs.noecho () # do not update terminal
+        cs.cbreak () # set direct mode (no enter needed)
+        self.io.keypad (True) # convert to key codes
+
+        with open (pm.sampleFileName, 'w') as self.sampleFile:
+            with sc.socket (*sw.socketType) as self.clientSocket:
+                self.clientSocket.connect (sw.address)
+                self.socketWrapper = sw.SocketWrapper (self.clientSocket)
+                self.halfApertureAngle = False
+
+                while True:
+                    self.input ()
+                    self.process ()
+                    self.output ()
+                    self.logTraining ()
+                    tm.sleep (0.02)
+
+    def input (self):
+        sensors = self.socketWrapper.recv ()
+
+        if not self.halfApertureAngle:
+            self.halfApertureAngle = sensors ['halfApertureAngle']
+            self.sectorAngle = 2 * self.halfApertureAngle / pm.lidarInputDim
+            self.halfMiddleApertureAngle = sensors ['halfMiddleApertureAngle']
+
+        if 'lidarDistances' in sensors:
+            self.lidarDistances = sensors ['lidarDistances']
+        else:
+            self.sonarDistances = sensors ['sonarDistances']
+
+    def process (self):
+        key = self.io.getch ()
+        if key == cs.KEY_UP:
+            self.targetVelocity += pm.speedDelta
+        elif key == cs.KEY_DOWN:
+            self.targetVelocity -= pm.speedDelta
+        elif key == cs.KEY_RIGHT:
+            self.steeringAngle += pm.steerDelta
+        elif key == cs.KEY_LEFT:
+            self.steeringAngle -= pm.steerDelta
+
+    def output (self):
+        actuators = {
+            'steeringAngle': self.steeringAngle,
+            'targetVelocity': self.targetVelocity
+        }
+
+        self.socketWrapper.send (actuators)
+
+    def logLidarTraining (self):
+        sample = [pm.finity for entryIndex in range (pm.lidarInputDim + 1)]
+
+        for lidarAngle in range (-self.halfApertureAngle, self.halfApertureAngle):
+            sectorIndex = round (lidarAngle / self.sectorAngle)
+            sample [sectorIndex] = min (sample [sectorIndex], self.lidarDistances [lidarAngle])
+
+        sample [-2] = self.steeringAngle
+        sample [-1] = self.targetVelocity
+        print (*sample, file = self.sampleFile)
+
+    def logSonarTraining (self):
+        sample = [pm.finity for entryIndex in range (pm.sonarInputDim + 1)]
+
+        for entryIndex, sectorIndex in ((2, -1), (0, 0), (1, 1)):
+            sample [entryIndex] = self.sonarDistances [sectorIndex]
+
+        sample [-2] = self.steeringAngle
+        sample [-1] = self.targetVelocity
+        print (*sample, file = self.sampleFile)
+
+    def logTraining (self):
+        if hasattr (self, 'lidarDistances'):
+            self.logLidarTraining ()
+        else:
+            self.logSonarTraining ()
+
+ManualClient ()

simpylc/simulations/car_manual_flex_track/control_client/parameters.py

@@ -0,0 +1,39 @@
+'''
+====== Legal notices
+
+Copyright (C) 2013 - 2021 GEATEC engineering
+
+This program is free software.
+You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense.
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY, without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+See the QQuickLicense for details.
+
+The QQuickLicense can be accessed at: http://www.qquick.org/license.html
+
+__________________________________________________________________________
+
+
+ THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !!
+
+__________________________________________________________________________
+
+It is meant for training purposes only.
+
+Removing this header ends your license.
+'''
+
+finity = 20.0   # Needs to be float to obtain ditto numpy array
+
+lidarInputDim = 16
+sonarInputDim = 3
+
+speedDelta = +0.2
+steerDelta = -5.0
+
+sampleFileName = 'default.samples'
+
+def getTargetVelocity (steeringAngle):
+    return (90 - abs (steeringAngle)) / 60

simpylc/simulations/car_manual_flex_track/control_server.py

@@ -0,0 +1,62 @@
+'''
+====== Legal notices
+
+Copyright (C) 2013 - 2021 GEATEC engineering
+
+This program is free software.
+You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense.
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY, without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+See the QQuickLicense for details.
+
+The QQuickLicense can be accessed at: http://www.qquick.org/license.html
+
+__________________________________________________________________________
+
+
+ THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !!
+
+__________________________________________________________________________
+
+It is meant for training purposes only.
+
+Removing this header ends your license.
+'''
+
+import socket as sc
+import time as tm
+
+import simpylc as sp
+import socket_wrapper as sw
+
+class ControlServer:
+    def __init__ (self):
+        with sc.socket (*sw.socketType) as serverSocket:
+            serverSocket.bind (sw.address)
+            serverSocket.listen (sw.maxNrOfConnectionRequests)
+
+            while True:
+                self.clientSocket = serverSocket.accept ()[0]
+                self.socketWrapper = sw.SocketWrapper (self.clientSocket)
+
+                with self.clientSocket:
+                    while True:
+                        sensors = {
+                            'halfApertureAngle': sp.world.visualisation.scanner.halfApertureAngle,
+                            'halfMiddleApertureAngle': sp.world.visualisation.scanner.halfMiddleApertureAngle
+                        } | (
+                                {'lidarDistances': sp.world.visualisation.scanner.lidarDistances}
+                            if hasattr (sp.world.visualisation.scanner, 'lidarDistances') else
+                                {'sonarDistances': sp.world.visualisation.scanner.sonarDistances}
+                        )
+
+                        self.socketWrapper.send (sensors)
+                        tm.sleep (0.02)
+                        actuators = self.socketWrapper.recv ()
+
+                        sp.world.physics.steeringAngle.set (actuators ['steeringAngle'])
+                        sp.world.physics.targetVelocity.set (actuators ['targetVelocity'])
+
+

simpylc/simulations/car_manual_flex_track/dimensions.py

@@ -0,0 +1,39 @@
+'''
+====== Legal notices
+
+Copyright (C) 2013 - 2021 GEATEC engineering
+
+This program is free software.
+You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense.
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY, without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+See the QQuickLicense for details.
+
+The QQuickLicense can be accessed at: http://www.qquick.org/license.html
+
+__________________________________________________________________________
+
+
+
+ THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !!
+
+__________________________________________________________________________
+
+It is meant for training purposes only.
+
+Removing this header ends your license.
+'''
+
+import simpylc as sp
+
+wheelDiameter = 0.10
+wheelRadius = wheelDiameter / 2
+displacementPerWheelAngle = sp.radiansPerDegree * wheelRadius
+
+wheelBase = 0.40
+wheelShift = wheelBase / 2
+
+apertureAngle = 120
+middleApertureAngle = 45

simpylc/simulations/car_manual_flex_track/easy.track

@@ -0,0 +1,32 @@
+- - - - - - - - - - - - - * * * - - - - - - - - - - - - - - - -
+- - - - - - - * * * * * * - - - * * * - - - - - - - - - - - - - 
+- - - - - - * - - - - - - - - - - - - * * * * - - - - - - - - -
+- - - - - * - - - - - - - - - - - - - - - - - * - - - - - - - -
+- - - - * - - - - - - - - * * * - - - - @ - - - * - - - - - - -
+- - - * - - - - * * * * * - - - * * * - - - - - - * - - - - - -
+- - * - - - - * - - - - - - - - - - - * * * - - - - * - - - - -
+- * - - - - * - - - - - - - - - - - - - - - * - - - - * - - - -
+* - - - - * - - - - - - - - - - - - - - - - - * - - - - * - - -
+* - - - * - - - - - - - - - - - - - - - - - - - * - - - - *  - -
+* - - - * - - - - - - - - - - - - - - - - - - - - * - - - - * -
+* - - - * - - - - - - - - - - - - - - - - - - - - - * - - - * -
+- * - - - * - - - - - - - - - - - - - - - - - - - - * - - - * -
+- * - - - * - - - - - - - - - - - - - - - - - - - - - * - - - *
+- * - - - * - - - - - - - - - - - - - - - - - - - - - * - - - *
+- - * - - - * - - - - - - - - - - - - - - - - - - - - * - - - *
+- - * - - - * - - - - - - - - - - - - - - - - - - - - * - - - *
+- - * - - - * - - - - - - - - - - - - - - - - - - - - * - - - *
+- * - - - * - - - - - - - - - - - - - - - - - - - - * - - - * -
+- * - - - * - - - - - - - - - - - - - - - - - - - * - - - * - -
+- * - - - * - - - - - - - - - - - - - - - - - - * - - - -*- - -
+* - - - * - - - - - - - - - - - - - - - - - - * - - - - * - - -
+* - - - * - - - - - - - - - - - - - - - - - * - - - - * - - - -
+* - - - * - - - - - - - - - - * * * * * * * - - - - * - - - - -
+- * - - - * - - - - - - - - * - - - - - - - - - - * - - - - - -
+- * - - - * - - - - - - - * - - - - - - - - - - * - - - - - - -
+- * - - - - * - - - - - * - - - - - - - - - - * - - - - - - - -
+- - * - - - - * - - - * - - - - * * * * * * * - - - - - - - - -
+- - - * - - - - * * * - - - - * - - - - - - - - - - - - - - - -
+- - - - * - - - - - - - - - * - - - - - - - - - - - - - - - - -
+- - - - - * - - - - - - - * - - - - - - - - - - - - - - - - - -
+- - - - - - * * * * * * * - - - - - - - - - - - - - - - - - - -

simpylc/simulations/car_manual_flex_track/hard.track

@@ -0,0 +1,32 @@
+- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
+- - - - - - - * - - - * - - - - - - - - - - - - - - - - - - - - 
+- - - - - - - - - - - - - - * - - - - - - - - - - - * - - - - -
+- - - * - - - * - - * - - - - - - - * - - - * - - - - - - - * -
+- - - - - * - - - - - - - * - - - - - - @ - - - - * - - - - - -
+- - - - - - - - - - - - - - - - - * - - - - * - - - - - * - - -
+- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
+- * - * - - - - - - - - - - - - - - - - - - - - - - - - - - * -
+- - - - - - - - - - - - - - - - - - - - - - - - - - - - * - - -
+- - - - - - - - - - * - - * - - - - - - - - - - - - - - - - - -
+- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
+- - * - * - - * - - - - * - - - - * - - - - - - - - - - * - * -
+- - - - - - - - - * - - - - - * - - - - - - - - - - - - - - - -
+- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
+- * - * - - - - - - - - - - - - - - - - - - - - - - - - - - - -
+- - - - - - - * - * - - - - - - - * - * - - - - - - - * - * - -
+- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
+- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
+- * - * - - - - * - * - - - - - - - - - - - - - - - - - - - - -
+- - - - - - - - - - - - - - - - - * - * - - - - - - * - * - - -
+- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
+- - - - - - - - - * - * - - - - - - - - - - - - - - - - - - - -
+- - * - * - - - - - - - - - - - - - - - - - - - - - - - - - - -
+- - - - - - - - - - - - - - - - - * - * - - - - - - * - * - - -
+- - - - - - - - - * - * - - - - - - - - - - - - - - - - - - - -
+- - - * - - - - - - - - - - - - - - - - - - - - - - - - - - - -
+- * - - - - - - - - - - - - - - - - - - * - - - - - - - - - - -
+- - - - * - - - - * - - - - - - - - * - - - - - - - - * - * - -
+- - - - - - * - * - - * - - - - - - - - - * - - - - - - - - - -
+- - * - - - - - - - - - - - - - - - - - - - - - * - * - - - - -
+- - - - - - * - - * - - - - - - - - - - * - - - - - - - * - - -
+- - - - - - - - - - - - - - - - - - - - - - - - * - - - - - - -