udp handshake

ROAR/agent_module/line_following_agent_2.py

@@ -4,7 +4,8 @@
 from ROAR.configurations.configuration import Configuration as AgentConfig
 import cv2
 import numpy as np
-from ROAR.control_module.simple_pid_controller import SimplePIDController
+# from ROAR.control_module.simple_pid_controller import SimplePIDController
+from ROAR.control_module.real_world_image_based_pid_controller import RealWorldImageBasedPIDController as PIDController
 from collections import deque
 from typing import List, Tuple, Optional
 
@@ -25,7 +26,7 @@ def __init__(self, vehicle: Vehicle, agent_settings: AgentConfig, **kwargs):
         # self.ycbcr_upper_range = (250, 240, 130)  # high range of color
         self.ycbcr_lower_range = (0, 180, 60)  # low range of color
         self.ycbcr_upper_range = (250, 240, 140)  # high range of color
-        self.controller = SimplePIDController(agent=self)
+        self.controller = PIDController(agent=self)
         self.prev_steerings: deque = deque(maxlen=10)
 
     def run_step(self, sensors_data: SensorsData, vehicle: Vehicle) -> VehicleControl:
@@ -53,10 +54,10 @@ def run_step(self, sensors_data: SensorsData, vehicle: Vehicle) -> VehicleContro
                                              error_scaling=[
                                                  (20, 0.1),
                                                  (40, 0.75),
-                                                 (60, 1.15),
-                                                 (80, 1.25),
-                                                 (100, 1.5),
-                                                 (200, 3)
+                                                 (60, 0.8),
+                                                 (80, 0.9),
+                                                 (100, 0.95),
+                                                 (200, 1)
                                              ])
             error_at_50 = self.find_error_at(data=data,
                                              y_offset=50,
@@ -67,32 +68,36 @@ def run_step(self, sensors_data: SensorsData, vehicle: Vehicle) -> VehicleContro
                                                  (40, 0.4),
                                                  (60, 0.6),
                                                  (70, 0.7),
-                                                 (80, 1),
-                                                 (100, 1.5),
-                                                 (200, 3)
+                                                 (80, 0.8),
+                                                 (100, 0.9),
+                                                 (200, 2)
                                              ]
                                              )
 
             if error_at_10 is None and error_at_50 is None:
                 # did not see the line
                 neutral = -90
                 incline = self.vehicle.transform.rotation.pitch - neutral
-                if incline > 0:
-                    # is flat or up slope, execute adjusted previous command
-                    return self.execute_prev_command()
-                else:
+                if incline < -10:
                     # is down slope, execute previous command as-is
                     # get the PID for downhill
                     long_control = self.controller.long_pid_control()
                     self.vehicle.control.throttle = long_control
                     return self.vehicle.control
 
+                else:
+                    # is flat or up slope, execute adjusted previous command
+                    return self.execute_prev_command()
+
+
             # we only want to follow the furthest thing we see.
             error = 0
             if error_at_10 is not None:
                 error = error_at_10
             if error_at_50 is not None:
                 error = error_at_50
+
+            print(error_at_10, error_at_50, error)
             self.kwargs["lat_error"] = error
             self.vehicle.control = self.controller.run_in_series()
             self.prev_steerings.append(self.vehicle.control.steering)
@@ -110,7 +115,7 @@ def find_error_at(self, data, y_offset, error_scaling, lower_range, upper_range)
         mask_yellow = cv2.inRange(src=data, lowerb=(0, 140, 0), upperb=(250, 200, 80))
         mask = mask_red | mask_yellow
 
-        # cv2.imshow("mask", mask)
+        cv2.imshow("mask", mask)
         # cv2.imshow("mask_red", mask_red)
         # cv2.imshow("mask_yellow", mask_yellow)
         kernel = np.ones((5, 5), np.uint8)
@@ -133,9 +138,10 @@ def find_error_at(self, data, y_offset, error_scaling, lower_range, upper_range)
         # we want small error to be almost ignored, only big errors matter.
         for e, scale in error_scaling:
             if abs(error) <= e:
-                # print(f"Error at {y_offset} -> {error, scale}")
+                print(f"Error at {y_offset} -> {error, scale} -> {error * scale}")
                 error = error * scale
                 break
+
         return error
 
     def execute_prev_command(self):
@@ -149,7 +155,7 @@ def execute_prev_command(self):
         # self.logger.info("Cannot see line, executing prev cmd")
         self.prev_steerings.append(self.vehicle.control.steering)
         self.vehicle.control.throttle = 0.18
-        # self.logger.info(f"No Lane found, executing discounted prev command: {self.vehicle.control}")
+        self.logger.info(f"No Lane found, executing discounted prev command: {self.vehicle.control}")
         return self.vehicle.control
 
     def rgb2ycbcr(self, im):

ROAR/configurations/iOS/iOS_agent_configuration.json

@@ -66,7 +66,7 @@
     ]
   },
   "waypoint_file_path": "./ROAR_Sim/data/easy_map_waypoints.txt",
-  "pid_config_file_path": "./ROAR/configurations/carla/carla_pid_config.json",
+  "pid_config_file_path": "./ROAR/configurations/iOS/iOS_pid_config.json",
   "simple_waypoint_local_planner_config_file_path": "./ROAR/configurations/carla/carla_simple_waypoint_local_planner_config.json",
   "depth_to_pcd_config_path": "./ROAR/configurations/iOS/iOS_depth_to_pcd_config.json",
   "occu_map_config_path": "./ROAR/configurations/iOS/iOS_occu_map_config.json",

ROAR/configurations/iOS/iOS_pid_config.json

@@ -1,41 +1,26 @@
 {
   "longitudinal_controller": {
-    "40": {
-          "Kp": 0.8,
-          "Kd": 0.4,
-          "Ki": 0
-    },
-    "60": {
-          "Kp": 0.5,
-          "Kd": 0.3,
-          "Ki": 0
-    },
-    "90": {
-          "Kp": 0.3,
-          "Kd": 0.3,
-          "Ki": 0
-    },
-    "150": {
-          "Kp": 0.2,
-          "Kd": 0.2,
-          "Ki": 0.1
-        }
+    "10": {
+          "Kp": 0.1,
+          "Kd": 0.0,
+          "Ki": 0.005
+    }
   },
   "latitudinal_controller": {
-    "60": {
-          "Kp": 0.8,
-          "Kd": 0.1,
-          "Ki": 0.1
+       "3": {
+          "Kp": 0.006,
+          "Kd": 0.075,
+          "Ki": 0.00025
     },
-    "100": {
-          "Kp": 0.6,
-          "Kd": 0.2,
-          "Ki": 0.1
+       "4": {
+          "Kp": 0.004,
+          "Kd": 0,
+          "Ki": 0
     },
-    "150": {
-          "Kp": 0.5,
-          "Kd": 0.2,
-          "Ki": 0.1
-        }
+           "6": {
+          "Kp": 0.002,
+          "Kd": 0,
+          "Ki": 0
+    }
   }
 }

ROAR/control_module/real_world_image_based_pid_controller.py

@@ -0,0 +1,87 @@
+import json
+
+from ROAR.control_module.controller import Controller
+from ROAR.utilities_module.data_structures_models import Transform
+from ROAR.utilities_module.vehicle_models import Vehicle
+from ROAR.utilities_module.vehicle_models import VehicleControl
+from collections import deque
+import numpy as np
+from ROAR_iOS.config_model import iOSConfig
+from pathlib import Path
+
+
+class RealWorldImageBasedPIDController(Controller):
+    def __init__(self, agent, **kwargs):
+        super().__init__(agent, **kwargs)
+        long_error_deque_length = 10
+        lat_error_deque_length = 10
+        self.lat_error_queue = deque(maxlen=lat_error_deque_length)  # this is how much error you want to accumulate
+        self.long_error_queue = deque(maxlen=long_error_deque_length)  # this is how much error you want to accumulate
+        self.target_speed = 10  # m / s
+        self.config = json.load(Path(self.agent.agent_settings.pid_config_file_path).open('r'))
+        self.long_config = self.config["longitudinal_controller"]
+        self.lat_config = self.config["latitudinal_controller"]
+
+    def run_in_series(self, next_waypoint=None, **kwargs) -> VehicleControl:
+        steering = self.lateral_pid_control()
+        throttle = self.long_pid_control()
+        return VehicleControl(throttle=throttle, steering=steering)
+
+    def lateral_pid_control(self) -> float:
+        error = self.agent.kwargs.get("lat_error", 0)
+        error_dt = 0 if len(self.lat_error_queue) == 0 else error - self.lat_error_queue[-1]
+        self.lat_error_queue.append(error)
+        error_it = sum(self.lat_error_queue)
+        k_p, k_d, k_i = self.find_k_values(self.agent.vehicle, self.lat_config)
+        # print(f"Speed = {self.agent.vehicle.get_speed(self.agent.vehicle)}"
+        #       f"kp, kd, ki = {k_p, k_d, k_i} ")
+
+        e_p = k_p * error
+        e_d = k_d * error_dt
+        e_i = k_i * error_it
+        lat_control = np.clip((e_p + e_d + e_i), -1, 1)
+        # print(f"speed = {self.agent.vehicle.get_speed(self.agent.vehicle)} "
+        #       f"e = {round((e_p + e_d + e_i), 3)}, "
+        #       f"e_p={round(e_p, 3)},"
+        #       f"e_d={round(e_d, 3)},"
+        #       f"e_i={round(e_i, 3)},"
+        #       f"lat_control={lat_control}")
+        # print()
+        return lat_control
+
+    def long_pid_control(self) -> float:
+        k_p, k_d, k_i = self.find_k_values(self.agent.vehicle, self.long_config)
+        e = self.target_speed - self.agent.vehicle.get_speed(self.agent.vehicle)
+        neutral = -90
+        incline = self.agent.vehicle.transform.rotation.pitch - neutral
+        e = e * - 1 if incline < -10 else e
+        self.long_error_queue.append(e)
+        de = 0 if len(self.long_error_queue) < 2 else self.long_error_queue[-2] - self.long_error_queue[-1]
+        ie = 0 if len(self.long_error_queue) < 2 else np.sum(self.long_error_queue)
+        incline = np.clip(incline, -20, 20)
+
+        e_p = k_p * e
+        e_d = k_d * de
+        e_i = k_i * ie
+        e_incline = 0.015 * incline
+        total_error = e_p + e_d + e_i + e_incline
+        long_control = np.clip(total_error, 0, 1)
+        # print(f"speed = {self.agent.vehicle.get_speed(self.agent.vehicle)} "
+        #       f"e = {round(total_error,3)}, "
+        #       f"e_p={round(e_p,3)},"
+        #       f"e_d={round(e_d,3)},"
+        #       f"e_i={round(e_i,3)},"
+        #       f"e_incline={round(e_incline, 3)}, "
+        #       f"long_control={long_control}")
+        return long_control
+
+    @staticmethod
+    def find_k_values(vehicle: Vehicle, config: dict) -> np.array:
+        current_speed = Vehicle.get_speed(vehicle=vehicle)
+        k_p, k_d, k_i = 1, 0, 0
+        for speed_upper_bound, kvalues in config.items():
+            speed_upper_bound = float(speed_upper_bound)
+            if current_speed < speed_upper_bound:
+                k_p, k_d, k_i = kvalues["Kp"], kvalues["Kd"], kvalues["Ki"]
+                break
+        return np.array([k_p, k_d, k_i])

ROAR/control_module/simple_pid_controller.py

@@ -23,7 +23,6 @@ def __init__(self, agent, **kwargs):
         # self.lat_kd = 0.075  # this is how much you want to resist change
         # self.lat_ki = 0.00025  # this is the correction on past error
 
-
         self.lat_kp = 0.005  # this is how much you want to steer
         self.lat_kd = 0.075  # this is how much you want to resist change
         self.lat_ki = 0.00025  # this is

handshake.py

@@ -0,0 +1,47 @@
+import logging
+from ROAR.utilities_module.utilities import get_ip
+import qrcode
+import cv2
+import numpy as np
+import socket
+
+
+def showIPUntilAckUDP():
+    img = np.array(qrcode.make(f"{get_ip()}").convert('RGB'))
+    success = False
+    addr = None
+
+    s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+    s.settimeout(0.1)
+
+    try:
+        s.bind((get_ip(), 8008))
+        while success is False:
+            try:
+                cv2.imshow("Scan this code to connect to phone", img)
+                k = cv2.waitKey(1) & 0xff
+                seg, addr = s.recvfrom(1024)  # this command might timeout
+
+                if k == ord('q') or k == 27:
+                    s.close()
+                    break
+                addr = addr
+                success = True
+                for i in range(10):
+                    print("data sent")
+                    s.sendto(b"hi", addr)
+            except socket.timeout as e:
+                logging.info(f"Please tap on the ip address to scan QR code. ({get_ip()}:{8008}). {e}")
+    except Exception as e:
+        logging.error(f"Unable to bind socket: {e}")
+    finally:
+        s.close()
+        cv2.destroyWindow("Scan this code to connect to phone")
+    return success, addr[0]
+
+
+logging.basicConfig(format='%(levelname)s - %(asctime)s - %(name)s '
+                           '- %(message)s',
+                    datefmt="%H:%M:%S",
+                    level=logging.DEBUG)
+print(showIPUntilAckUDP())

handshake_server.py

@@ -1,27 +1,13 @@
-from pathlib import Path
-from ROAR_iOS.ios_runner import iOSRunner
-from ROAR.configurations.configuration import Configuration as AgentConfig
-from ROAR_iOS.config_model import iOSConfig
-from ROAR_Unity.unity_runner import iOSUnityRunner
-# from ROAR.agent_module.ios_agent import iOSAgent
-# from ROAR.agent_module.free_space_auto_agent import FreeSpaceAutoAgent
-# from ROAR.agent_module.line_following_agent_2 import LineFollowingAgent
-from ROAR.agent_module.special_agents.recording_agent import RecordingAgent
-from ROAR.agent_module.traffic_light_detector_agent import TrafficLightDectectorAgent
-from ROAR.agent_module.aruco_following_agent import ArucoFollowingAgent
-from ROAR.agent_module.udp_multicast_agent import UDPMultiCastAgent
-from ROAR.agent_module.forward_only_agent import ForwardOnlyAgent
-from ROAR.utilities_module.vehicle_models import Vehicle
 import logging
-import argparse
-from misc.utils import str2bool
 from ROAR.utilities_module.utilities import get_ip
 import qrcode
 import cv2
 import numpy as np
 import socket
-import json
-import requests
+from ROAR.utilities_module.utilities import get_ip
+
+import socket
+
 
 def showIPUntilAck():
     img = np.array(qrcode.make(f"{get_ip()}").convert('RGB'))
@@ -58,4 +44,5 @@ def showIPUntilAck():
         cv2.destroyWindow("Scan this code to connect to phone")
     return success, addr
 
-print(showIPUntilAck())
+
+print(showIPUntilAck())