added init lanekeeping and mntctrl

.gitignore

@@ -1,3 +1,6 @@
+# vscode
+.vscode 
+
 # Byte-compiled / optimized / DLL files
 __pycache__/
 *.py[cod]

Brain/go.py

@@ -0,0 +1,147 @@
+# Copyright (c) 2019, Bosch Engineering Center Cluj and BFMC orginazers
+# All rights reserved.
+
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+
+# 1. Redistributions of source code must retain the above copyright notice, this
+#    list of conditions and the following disclaimer.
+
+# 2. Redistributions in binary form must reproduce the above copyright notice,
+#    this list of conditions and the following disclaimer in the documentation
+#    and/or other materials provided with the distribution.
+
+# 3. Neither the name of the copyright holder nor the names of its
+#    contributors may be used to endorse or promote products derived from
+#    this software without specific prior written permission.
+
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+# ========================================================================
+# SCRIPT USED FOR WIRING ALL COMPONENTS
+# ========================================================================
+import sys
+
+sys.path.append(".")
+
+import time
+import signal
+from multiprocessing import Pipe, Process, Event
+
+# hardware imports
+from src.hardware.camera.cameraprocess import CameraProcess
+from src.hardware.camera.CameraSpooferProcess import CameraSpooferProcess
+from src.hardware.serialhandler.SerialHandlerProcess import SerialHandlerProcess
+
+# utility imports
+from src.utils.camerastreamer.CameraStreamerProcess import CameraStreamerProcess
+from src.utils.remotecontrol.RemoteControlReceiverProcess import (
+    RemoteControlReceiverProcess,
+)
+from src.utils.controlsys.lanekeeping import LaneKeeping
+from src.utils.controlsys.momentcontrol import MovementControl
+
+# =============================== CONFIG =================================================
+enableStream = True
+enableCameraSpoof = False
+enableRc = True
+enableLaneKeeping = False
+# =============================== INITIALIZING PROCESSES =================================
+allProcesses = list()
+# Pipe collections
+movementControlR = list()
+camOutPs = list()
+
+# =============================== DATA ===================================================
+# LocSys client process
+# LocStR, LocStS = Pipe(duplex = False)           # LocSys  ->  brain
+# from data.localisationsystem.locsys import LocalisationSystemProcess
+# LocSysProc = LocalisationSystemProcess([], [LocStS])
+# allProcesses.append(LocSysProc)
+
+
+# =============================== CONTROL =================================================
+if enableRc:
+    rcShR, rcShS = Pipe(duplex=False)  # rc      ->  serial handler
+
+    # serial handler process
+    shProc = SerialHandlerProcess([rcShR], [])
+    allProcesses.append(shProc)
+
+    rcProc = RemoteControlReceiverProcess([], [rcShS])
+    allProcesses.append(rcProc)
+
+
+# ===================================== LANE KEEPING  ===================================
+if enableLaneKeeping:
+    # Pipes:
+
+    # Camera process -> Lane keeping
+    lkR, lkS = Pipe(duplex = False)
+
+    # Lane keeping -> Movement control
+    lcR, lcS = Pipe(duplex = False)
+
+    # Movement control -> Serial handler
+    cfR, cfS = Pipe(duplex = False)
+    camOutPs.append(lkS)
+    movementControlR.append(lcR)
+    lkProc = LaneKeeping([lkR], [lcS])
+    allProcesses.append(lkProc)
+
+    # Movement control
+    cfProc = MovementControl(movementControlR, [cfS])
+    allProcesses.append(cfProc)
+
+    # Serial handler
+    shProc = SerialHandlerProcess([cfR], [])
+    allProcesses.append(shProc)
+
+# =============================== HARDWARE ===============================================
+if enableStream:
+    camStR, camStS = Pipe(duplex=False)  # camera  ->  streamer
+    camOutPs.append(camStS)
+    if enableCameraSpoof:
+        camSpoofer = CameraSpooferProcess([], [camStS], "vid")
+        allProcesses.append(camSpoofer)
+
+    else:
+        camProc = CameraProcess([], camOutPs)
+        allProcesses.append(camProc)
+
+    streamProc = CameraStreamerProcess([camStR], [])
+    allProcesses.append(streamProc)
+
+
+# ===================================== START PROCESSES ==================================
+print("Starting the processes!", allProcesses)
+for proc in allProcesses:
+    proc.daemon = True
+    proc.start()
+
+
+# ===================================== STAYING ALIVE ====================================
+blocker = Event()
+
+try:
+    blocker.wait()
+except KeyboardInterrupt:
+    print("\nCatching a KeyboardInterruption exception! Shutdown all processes.\n")
+    for proc in allProcesses:
+        if hasattr(proc, "stop") and callable(getattr(proc, "stop")):
+            print("Process with stop", proc)
+            proc.stop()
+            proc.join()
+        else:
+            print("Process witouth stop", proc)
+            proc.terminate()
+            proc.join()

Brain/src/utils/controlsys/lanekeeping.py

@@ -0,0 +1,148 @@
+import socket
+import struct
+import time
+import numpy as np
+import datetime
+import cv2
+import math
+
+from multiprocessing    import Process
+from threading          import Thread
+from simple_pid         import PID
+
+from templates.workerprocess import WorkerProcess
+
+class LaneKeeping(WorkerProcess):
+    pid = PID(Kp = 1.0, Ki = 1.45, Kd = 0.15)
+
+    # ===================================== Worker process =========================================
+    def __init__(self, inPs, outPs):
+        """Process used for the image processing needed for lane keeping and for computing the steering value.
+        
+        Parameters
+        ----------
+        inPs : list(Pipe) 
+            List of input pipes (0 - receive image feed from the camera)
+        outPs : list(Pipe) 
+            List of output pipes (0 - send steering data to the movvement control process)
+        """
+        super(LaneKeeping,self).__init__(inPs, outPs)
+
+    def run(self):
+        """Apply the initializing methods and start the threads.
+        """
+        super(LaneKeeping,self).run()
+
+    def _init_threads(self):
+        """Initialize the thread.
+        """
+        if self._blocker.is_set():
+            return
+
+        thr = Thread(name='StreamSending',target = self._the_thread, args= (self.inPs[0], self.outPs[0], ))
+        thr.daemon = True
+        self.threads.append(thr)
+
+
+    # ===================================== Custom methods =========================================
+    def laneKeeping(self, img):
+        """Applies required image processing. 
+        
+        Parameters
+        ----------
+        img : Pipe
+            The image on which to apply the algorithm.
+        """
+        # Image dimensions
+        height = img.shape[0]
+        width = img.shape[1]
+
+        # Transform to grayscale
+        img = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
+
+        # Crop to obtain region of interest
+        img = img[(int(height/1.8)):height, 0:width]
+
+        # Blur to remove the noise
+        img = cv2.GaussianBlur(img, (7,7), 0)
+
+        # Apply adaptive threshold to obtain the road markings
+        img = cv2.adaptiveThreshold(img, 255, cv2.ADAPTIVE_THRESH_MEAN_C, cv2.THRESH_BINARY, 21, -8)
+
+        # Keep only a trapezoid containing the lines of the current lane
+        region_of_interest_vertices = [
+                (0, height - 1),
+                (0.25*width, 0),
+                (0.75*width, 0),
+                (width - 1, height - 1),
+        ]
+
+        def region_of_interest(img, vertices):
+            mask = np.zeros_like(img)
+            match_mask_color = (255,)
+            cv2.fillPoly(mask, vertices, match_mask_color)
+            masked_image = cv2.bitwise_and(img, mask)
+            return masked_image
+
+        img = region_of_interest(
+            img,
+            np.array([region_of_interest_vertices], np.int32),
+        )
+
+        # Compute the hough lines from the image
+        total = 0.0
+        lines = cv2.HoughLinesP(img, rho=6, theta=np.pi/60, threshold=160, lines=np.array([]), minLineLength=40, maxLineGap=25)
+
+        # Compute the sum of the slopes of the hough lines
+        for line in lines:
+            for x1, y1, x2, y2 in line:
+                if y2 != y1:
+                    total = total + (x2 - x1) / (y2 - y1)
+                else: return 0.0
+
+        return total
+
+    def computeSteeringAngle(self, val):
+        # Apply pid
+        newVal = self.pid(val)
+
+        # Calibrate result
+        newVal = val / 2.9
+
+        newVal = -newVal
+
+        newVal += 3.1
+
+        return newVal
+
+    def _the_thread(self, inP, outP):
+        """Obtains image, applies the required image processing and computes the steering angle value. 
+        
+        Parameters
+        ----------
+        inP  : Pipe
+            Input pipe to read the frames from other process.
+        outP : Pipe
+            Output pipe to send the steering angle value to other process.
+        """
+        while True:
+            try:
+                # Obtain image
+                stamps, img = inP.recv()
+
+                # Apply image processing
+                val = self.laneKeeping(img)
+
+                # Compute steering angle
+                val = self.computeSteeringAngle(val)
+
+                # Print steering angle value
+                #print(val)
+
+                # Send steering angle value
+                outP.send(val)
+
+            except Exception as e:
+                #print("Lane keeping error:")
+                #print(e)
+                1==1