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