Merge pull request #504 from RoboCupULaval/dev-montreal

Handle with Care [Explosive]

Engine/Communication/receiver/receiver_base_class.py

@@ -1,4 +1,5 @@
 import os
+import socket
 from abc import ABCMeta, abstractmethod
 from multiprocessing import Process, Queue
 from multiprocessing.managers import DictProxy
@@ -45,9 +46,6 @@ def run(self):
             pass
         except:
             self.logger.exception('An error occurred.')
-            raise
-
-    def terminate(self):
-        self.connection.close()
-        self.logger.debug('Terminated')
-        super().terminate()
+        finally:
+            self.connection.close()
+            self.logger.debug('Closed.')

Engine/Communication/receiver/uidebug_command_receiver.py

@@ -4,7 +4,8 @@
 from pickle import loads
 from queue import Full
 
-from socket import socket, AF_INET, SOCK_DGRAM, IPPROTO_IP, IP_ADD_MEMBERSHIP, inet_aton, INADDR_ANY
+from socket import socket, AF_INET, SOCK_DGRAM, IPPROTO_IP, IP_ADD_MEMBERSHIP, inet_aton, INADDR_ANY, SOL_SOCKET, \
+    SO_REUSEADDR
 from struct import pack
 
 from Engine.Communication.receiver.receiver_base_class import ReceiverProcess
@@ -16,12 +17,14 @@ class UIDebugCommandReceiver(ReceiverProcess):
 
     def connect(self, connection_info):
         connection = socket(AF_INET, SOCK_DGRAM)
+        connection.setsockopt(SOL_SOCKET, SO_REUSEADDR, 1)
         connection.bind(connection_info)
         if ip_address(connection_info[0]).is_multicast:
             connection.setsockopt(IPPROTO_IP,
                                   IP_ADD_MEMBERSHIP,
                                   pack("=4sl", inet_aton(connection_info[0]), INADDR_ANY))
 
+
         return connection
 
     def receive_packet(self):

Engine/Communication/sender/sender_base_class.py

@@ -1,4 +1,5 @@
 import os
+import socket
 from abc import ABCMeta, abstractmethod
 from multiprocessing import Process, Queue
 
@@ -42,10 +43,6 @@ def run(self):
             pass
         except:
             self.logger.exception('An error occurred.')
-            raise
-
-    def terminate(self):
-        self.logger.debug('Terminated')
-        self._queue.close()
-        self.connection.close()
-        super().terminate()
+        finally:
+            self.connection.close()
+            self.logger.debug('Closed.')

Engine/Communication/sender/serial_command_sender.py

@@ -3,7 +3,10 @@
 from pyhermes import McuCommunicator
 
 from Engine.Communication.sender.sender_base_class import Sender
+from Engine.robot import MAX_LINEAR_SPEED, MAX_ANGULAR_SPEED
 from Util.constant import KickForce, DribbleState
+from Util.geometry import clamp
+import numpy as np
 
 
 class SerialCommandSender(Sender):
@@ -14,10 +17,17 @@ def connect(self, connection_info):
     def send_packet(self, packets_frame):
         try:
             for packet in packets_frame.packet:
+                if np.isnan(packet.command.x) or \
+                    np.isnan(packet.command.y) or \
+                    np.isnan(packet.command.orientation):
+                    continue
+                cx = clamp(packet.command.x, -MAX_LINEAR_SPEED, MAX_LINEAR_SPEED)
+                cy = clamp(packet.command.y, -MAX_LINEAR_SPEED, MAX_LINEAR_SPEED)
+                orien = clamp(packet.command.orientation, -MAX_ANGULAR_SPEED, MAX_ANGULAR_SPEED)
                 self.connection.sendSpeedAdvance(packet.robot_id,
-                                                 packet.command.x/1000,
-                                                 packet.command.y/1000,
-                                                 packet.command.orientation,
+                                                 cx/1000,
+                                                 cy/1000,
+                                                 orien,
                                                  packet.charge_kick,
                                                  self.translate_kick_force(packet.kick_force),
                                                  self.translate_dribbler_speed(packet.dribbler_state))

Engine/Framework.py

@@ -1,9 +1,11 @@
 # Under MIT License, see LICENSE.txt
 
 import logging
+import os
+import signal
 from multiprocessing import Queue, Manager
 import sys
-from time import time
+import time
 
 from Engine.engine import Engine
 from ai.coach import Coach
@@ -29,10 +31,9 @@ def __init__(self, profiling=False):
         self.ui_send_queue = Queue(maxsize=Framework.QUEUE_SIZE)
         self.ui_recv_queue = Queue(maxsize=Framework.QUEUE_SIZE)
 
-        self.engine_watchdog = Manager().Value('f', time())
+        self.engine_watchdog = Manager().Value('f', time.time())
         self.engine = Engine(self)
-
-        self.coach_watchdog = Manager().Value('f', time())
+        self.coach_watchdog = Manager().Value('f', time.time())
         self.coach = Coach(self)
 
     def start(self):
@@ -43,21 +44,31 @@ def start(self):
         sleep = create_fps_timer(Framework.CHECK_SUBPROCESS_STATE_IN_SECONDS)
 
         try:
-            while self.engine.is_alive() and self.coach.is_alive():
+            while self.coach.is_alive() and self.engine.is_alive():
                 sleep()
 
         except SystemExit:
-            self.logger.debug('Terminated')
+            pass
         except KeyboardInterrupt:
-            self.logger.debug('A keyboard interrupt was raise.')
+            self.logger.debug('Interrupted.')
         except BrokenPipeError:
-            self.logger.info('A connection was broken.')
+            self.logger.exception('A connection was broken.')
         except:
             self.logger.exception('An error occurred.')
         finally:
             self.stop_game()
 
     def stop_game(self):
-        self.engine.terminate()
-        self.coach.terminate()
+        self.logger.critical('Framework stopped.')
+
+        try:
+            os.kill(self.engine.pid, signal.SIGINT)
+        except ProcessLookupError:
+            pass
+
+        try:
+            os.kill(self.coach.pid, signal.SIGINT)
+        except ProcessLookupError:
+            pass
+
         sys.exit()

Engine/controller.py

@@ -57,13 +57,13 @@ def execute(self, dt: float) -> RobotState:
 
         for robot in self.active_robots:
             robot.path, robot.target_speed = path_smoother(robot.raw_path, robot.cruise_speed, robot.end_speed)
-
-            if robot.target_speed < 10:
-                commands[robot.id] = min(robot.velocity_regulator.execute(robot, dt),
-                                         robot.position_regulator.execute(robot, dt),
-                                         key=lambda cmd: cmd.norm)
-            else:
-                commands[robot.id] = robot.velocity_regulator.execute(robot, dt)
+            #
+            # if robot.target_speed < 10 and False:
+            #     commands[robot.id] = min(robot.velocity_regulator.execute(robot, dt),
+            #                              robot.position_regulator.execute(robot, dt),
+            #                              key=lambda cmd: cmd.norm)
+            # else:
+            commands[robot.id] = robot.velocity_regulator.execute(robot, dt)
 
         self.send_debug(commands)
 

Engine/engine.py

@@ -6,7 +6,7 @@
 from multiprocessing import Process, Manager
 from queue import Empty
 
-from time import time
+from time import time, sleep
 
 from Debug.debug_command_factory import DebugCommandFactory
 
@@ -75,9 +75,7 @@ def callback(excess_time):
         self.fps_sleep = create_fps_timer(self.fps, on_miss_callback=callback)
 
         # profiling
-        self.profiler = cProfile.Profile()
-        if self.framework.profiling:
-            self.profiler.enable()
+        self.profiler = None
 
     def start(self):
         super().start()
@@ -88,15 +86,21 @@ def start(self):
 
     def run(self):
 
-        logged_string = 'Running with process ID {}'.format(os.getpid())
-        if self.is_fps_locked:
-            logged_string += ' at {} fps.'.format(self.fps)
-        else:
-            logged_string += ' without fps limitation.'
+        try:
+
+            logged_string = 'Running with process ID {}'.format(os.getpid())
+            if self.is_fps_locked:
+                logged_string += ' at {} fps.'.format(self.fps)
+            else:
+                logged_string += ' without fps limitation.'
+
+            self.logger.debug(logged_string)
+
+            self.profiler = cProfile.Profile()
+            if self.framework.profiling:
+                self.profiler.enable()
 
-        self.logger.debug(logged_string)
 
-        try:
             self.wait_for_vision()
             self.last_time = time()
             while True:
@@ -105,12 +109,15 @@ def run(self):
                 self.main_loop()
                 if self.is_fps_locked: self.fps_sleep()
                 self.framework.engine_watchdog.value = time()
+
         except KeyboardInterrupt:
-            pass
+            self.logger.debug('Interrupted.')
         except BrokenPipeError:
-            self.logger.info('A connection was broken.')
+            self.logger.exception('A connection was broken.')
         except:
             self.logger.exception('An error occurred.')
+        finally:
+            self.stop()
 
     def wait_for_vision(self):
         self.logger.debug('Waiting for vision frame from the VisionReceiver...')
@@ -125,7 +132,6 @@ def update_time(self):
 
     def main_loop(self):
         engine_cmds = self.get_engine_commands()
-
         game_state = self.tracker.update()
         self.game_state.update(game_state)
 
@@ -151,9 +157,8 @@ def get_engine_commands(self):
 
     def dump_profiling_stats(self):
         if self.framework.profiling:
-            if self.frame_count % (self.fps * config['ENGINE']['profiling_dump_time']) == 0:
-                self.profiler.dump_stats(config['ENGINE']['profiling_filename'])
-                self.logger.debug('Profiling data written to {}.'.format(config['ENGINE']['profiling_filename']))
+            self.profiler.dump_stats(config['ENGINE']['profiling_filename'])
+            self.logger.debug('Profiling data written to {}.'.format(config['ENGINE']['profiling_filename']))
 
     def is_alive(self):
 
@@ -168,11 +173,6 @@ def is_alive(self):
                                         self.referee_recver.is_alive()))
         return borked_process_not_found and super().is_alive()
 
-    def terminate(self):
+    def stop(self):
         self.dump_profiling_stats()
-        self.vision_receiver.terminate()
-        self.ui_sender.terminate()
-        self.ui_recver.terminate()
-        self.referee_recver.terminate()
-        self.logger.debug('Terminated')
-        super().terminate()
+        self.logger.debug('Stopped.')

Engine/filters/robot_kalman_filter.py

@@ -43,8 +43,8 @@ def control_input_model(self, dt):
                          [0,  0, dt]])  # Speed Theta
 
     def process_covariance(self, dt):
-        sigma_acc_x = 500
-        sigma_acc_y = 500
+        sigma_acc_x = 1000
+        sigma_acc_y = 1000
         sigma_acc_o = 20000 * np.pi/180
 
         process_covariance = \

Engine/regulators/velocity_regulators.py

@@ -13,7 +13,7 @@
 class RealVelocityController(RegulatorBaseClass):
 
     settings = {'kp': 10, 'ki': 0, 'kd': 1}
-    v_d = 5  # lower = bigger path correction
+    v_d = 4  # lower = bigger path correction
     emergency_break_constant = 0.4  # Higher = higher correction of trajectory
     emergency_break_safety_factor = 1  # lower = bigger break distance
 
@@ -46,7 +46,7 @@ def following_path_vector(self, robot):
             return direction_error
 
     def get_next_speed(self, robot, acc=MAX_LINEAR_ACCELERATION):
-        acceleration_offset = 1.5  # on veut que le robot soit plus aggressif en début de trajet
+        acceleration_offset = 1  # on veut que le robot soit plus aggressif en début de trajet
         emergency_break_offset = self.emergency_break_constant / self.dt * (robot.current_speed / 1000)  # on veut que le robot break le plus qu'il peut si on s'approche trop vite de la target
         emergency_break_offset = max(1.0, emergency_break_offset)
 
@@ -78,7 +78,7 @@ class GrSimVelocityController(RealVelocityController):
     settings = {'kp': 2, 'ki': 0.3, 'kd': 0}
     v_d = 15
     emergency_break_constant = 0
-    emergency_break_safety_factor = 1
+    emergency_break_safety_factor = 1  # lower = bigger break distance
 
 
 def is_time_to_break(robot, destination, cruise_speed, acceleration, target_speed):

Engine/robot.py

@@ -5,7 +5,7 @@
 
 MAX_LINEAR_SPEED = 4000  # mm/s
 MAX_LINEAR_ACCELERATION = 4000  # mm/s^2
-MAX_ANGULAR_SPEED = 50  # rad/s
+MAX_ANGULAR_SPEED = 70  # rad/s
 MAX_ANGULAR_ACC = 3  # rad/s^2
 MIN_LINEAR_SPEED = 200  # mm/s Speed near zero, but still move the robot
 

Engine/tracker.py

@@ -21,7 +21,7 @@
 
 class Tracker:
 
-    MAX_BALLS_SEPARATION = 2000
+    MAX_BALLS_SEPARATION = 1000
 
     def __init__(self, vision_state: DictProxy, ui_send_queue: Queue):
         self.logger = logging.getLogger(self.__class__.__name__)
@@ -97,7 +97,6 @@ def _remove_undetected(self):
                                   config['ENGINE']['max_undetected_robot_time'],
                                   undetected_robots)
 
-
         for ball in self.active_balls:
             if time() - ball.last_update_time > config['ENGINE']['max_undetected_ball_time']:
                 ball.reset()
@@ -120,6 +119,7 @@ def _log_new_robots_on_field(self, detection_frame: Dict[str, List[Dict[str, Any
 
         for robot_obs in detection_frame.get('robots_blue', ()):
             if not self._blue_team[robot_obs['robot_id']].is_active: new_robots['blue'].add(robot_obs['robot_id'])
+
         if new_robots['blue']:
             self.logger.debug('Blue robot(s) detected: %r', new_robots['blue'])
 

Util/constant.py

@@ -6,9 +6,8 @@
 
 ROBOT_RADIUS = 90
 ROBOT_DIAMETER = ROBOT_RADIUS * 2
-ROBOT_CENTER_TO_KICKER = 50
+ROBOT_CENTER_TO_KICKER = 60
 BALL_RADIUS = 21
-PLAYER_PER_TEAM = 12
 MAX_PLAYER_ON_FIELD_PER_TEAM = 6
 BALL_OUTSIDE_FIELD_BUFFER = 200
 
@@ -59,7 +58,6 @@ def for_dist(cls, dist):
             return KickForce.HIGH
 
 
-
 class KickType(Enum):
     DIRECT = 0
     CHIP = 1

Util/geometry.py

@@ -126,6 +126,14 @@ def from_limits(cls, top, bottom, right, left):
     def flip_x(cls, area):
         return Area.from_limits(area.top, area.bottom, -area.left, -area.right)
 
+    @classmethod
+    def from_4_point(cls, p1, p2, p3, p4):
+        top = max(p1.y, p2.y, p3.y, p4.y)
+        bot = min(p1.y, p2.y, p3.y, p4.y)
+        right = max(p1.x, p2.x, p3.x, p4.x)
+        left = min(p1.x, p2.x, p3.x, p4.x)
+        return Area.from_limits(top, bot, right, left)
+
 
 def find_bisector_of_triangle(c, a, b):
     """