From c669361447fdd35297949705e1e56853e5ac1ea6 Mon Sep 17 00:00:00 2001
From: Armin <armin.co@hs-bochum.de>
Date: Mon, 8 Feb 2021 00:31:59 +0100
Subject: [PATCH] Added Collistion sensor

---
 carla_environment.py | 45 ++++++++++++++++++++++++++++++++++++++++++--
 steering_wheel.py    | 21 +++++++++++----------
 2 files changed, 54 insertions(+), 12 deletions(-)

diff --git a/carla_environment.py b/carla_environment.py
index 915a157..2eeb376 100644
--- a/carla_environment.py
+++ b/carla_environment.py
@@ -3,6 +3,8 @@ import glob
 import os
 import random
 import sys
+import math
+import weakref
 
 import numpy as np
 import pygame
@@ -64,13 +66,34 @@ class Camera:
             self.display.blit(self.surface, (0, 0))
             pygame.display.flip()
 
+class CollisionSensor:
+    sensor = None
+
+    def __init__(self, world, parent):
+        bp = world.get_blueprint_library().find('sensor.other.collision')
+        self.sensor = world.spawn_actor(bp, carla.Transform(), attach_to=parent)
+        weak_self = weakref.ref(self)
+        self.sensor.listen(lambda event: CollisionSensor._on_collision(weak_self, event))
+        
+    def _on_collision(weak_self, event):
+        self = weak_self()
+        if not self:
+            return
+        print(event.other_actor)
+        impulse = event.normal_impulse
+        intensity = math.sqrt(impulse.x**2 + impulse.y**2 + impulse.z**2)
+        print(intensity)
+
+
 
 class World:
     """ Wrapper for the carla environment, incl. player/vehicle """
     player = None
+    collision_sensor = None
     world = None
     blueprint_library = None
     spawn_points = None
+    actors = []
 
     def __init__(self, world):
         self.world = world
@@ -82,28 +105,46 @@ class World:
         """ Remove and create new player/vehicle. """
         self.destroy()
         self.spawn_player()
+        self.spawn_actors()
 
     def spawn_player(self):
         """ Add a vehicle to the world. """
         while self.player is None:
             blueprint = random.choice(self.blueprint_library.filter('model3'))
-            position = random.choice(self.spawn_points)
+            position = carla.Transform(carla.Location(x=-13, y=57.5, z=0.2), carla.Rotation(pitch=0.0, yaw=-180, roll=0.0))
             self.player = self.world.try_spawn_actor(blueprint, position)
             start_location = self.player.get_location()
             print(str(start_location))
+            self.collision_sensor = CollisionSensor(self.world, self.player)
             # start_location.x = 288.0
             # start_location.y = 55.0
             # self.player.set_location(start_location)
 
+    def spawn_actors(self):
+        blueprint = random.choice(self.blueprint_library.filter('etron'))
+        transforms = []
+        transforms.append(carla.Transform(carla.Location(x=-19.2, y=54.5, z=0.2), carla.Rotation(pitch=0.0, yaw=-180, roll=0.0)))
+        transforms.append(carla.Transform(carla.Location(x=-14.3, y=54.7, z=0.2), carla.Rotation(pitch=0.0, yaw=-180, roll=0.0)))
+        transforms.append(carla.Transform(carla.Location(x=-41.3, y=53.7, z=0.2), carla.Rotation(pitch=0.0, yaw=-180, roll=0.0)))
+
+        for transform in transforms:
+            actor = self.world.try_spawn_actor(blueprint, transform)
+            if actor is not None:
+                self.actors.append(actor)
+
     def destroy(self):
         """ Remove vehicle from the world. """
         if self.player is not None:
             self.player.destroy()
+        if self.actors is not None:
+            for actor in self.actors:
+                actor.destroy()
+
 
     def step(self, action):
         """ Apply controls to vehicle. """
         self.player.apply_control(action)
-        # print(str(self.player.get_location()))
+        # print(str(self.player.get_transform()))
         # print(str(self.player.get_velocity()))
 
 
diff --git a/steering_wheel.py b/steering_wheel.py
index 92f5d8a..66e031e 100644
--- a/steering_wheel.py
+++ b/steering_wheel.py
@@ -2,12 +2,12 @@
 
 import pygame
 
-DIRECTION=0
-THROTTLE=1
-BRAKES=2
-INPUTS=3
+JOYSTICK_DIRECTION=0
+JOYSTICK_THROTTLE=1
+JOYSTICK_BRAKES=2
+JOYSTICK_REVERSE=3
 
-OFFSET = 2.0
+INPUT_OFFSET = 2.0
 
 class ManualSteeringWheel:
     """ Steering wheel """
@@ -18,6 +18,7 @@ class ManualSteeringWheel:
     brakes = 0.0
     reverse = False
 
+
     def __init__(self):
         self.joystick = pygame.joystick.Joystick(0)
         self.joystick.init()
@@ -27,23 +28,22 @@ class ManualSteeringWheel:
         """ Update and return direction. """
         if update:
             pygame.event.get()
-        self.direction = self.joystick.get_axis(DIRECTION)
+        self.direction = self.joystick.get_axis(JOYSTICK_DIRECTION)
         return self.direction
     
     def get_throttle(self, update=True):
         """ Update and return throttle."""
         if update:
             pygame.event.get()
-        self.throttle = (OFFSET - (self.joystick.get_axis(THROTTLE) + 1.0)) / OFFSET
+        self.throttle = round((INPUT_OFFSET - (self.joystick.get_axis(JOYSTICK_THROTTLE) + 1.0)) / INPUT_OFFSET, 3) 
         return self.throttle
 
     def get_brakes(self, update=True):
         """ Update and return brakes. """
         if update:
             pygame.event.get()
-        self.brakes = (OFFSET - (self.joystick.get_axis(BRAKES) + 1.0)) / OFFSET
-        if self.brakes < 0.01:
-            self.brakes = 0.0
+        self.brakes = (INPUT_OFFSET - (self.joystick.get_axis(JOYSTICK_BRAKES) + 1.0)) / INPUT_OFFSET
+        self.brakes = round(self.brakes, 3)
         return self.brakes
 
     def update_controls(self):
@@ -53,6 +53,7 @@ class ManualSteeringWheel:
         self.get_brakes(update=False)
 
     def get_action(self):
+
         return [self.throttle, self.brakes, self.direction, self.reverse]
 
 
-- 
GitLab