diff --git a/build/docker-compose.yml b/build/docker-compose.yml index 1545f945..a44d725e 100644 --- a/build/docker-compose.yml +++ b/build/docker-compose.yml @@ -57,8 +57,9 @@ services: tty: true shm_size: 2gb #command: bash -c "sleep 10 && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=/opt/leaderboard/data/routes_devtest.xml --agent=/opt/leaderboard/leaderboard/autoagents/npc_agent.py --host=carla-simulator --track=SENSORS" - command: bash -c "sleep 10 && roslaunch agent/launch/dev.launch" - #command: bash -c "sleep 10 && sudo chown -R carla:carla ../code/ && sudo chmod -R a+w ../code/ && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=/opt/leaderboard/data/routes_devtest.xml --agent=/workspace/code/agent/src/agent/agent.py --host=carla-simulator --track=MAP" + # command: bash -c "sleep 10 && roslaunch agent/launch/dev.launch" + command: bash -c "sleep 10 && sudo chown -R carla:carla ../code/ && sudo chmod -R a+w ../code/ && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=/opt/leaderboard/data/routes_devtest.xml --agent=/workspace/code/agent/src/agent/agent.py --host=carla-simulator --track=MAP" + logging: driver: "local" environment: diff --git a/code/acting/launch/acting.launch b/code/acting/launch/acting.launch index 5a90b88a..586101e7 100644 --- a/code/acting/launch/acting.launch +++ b/code/acting/launch/acting.launch @@ -29,10 +29,10 @@ - + diff --git a/code/planning/global_planner/launch/global_planner.launch b/code/planning/global_planner/launch/global_planner.launch index 6a13825f..cdb6dec9 100644 --- a/code/planning/global_planner/launch/global_planner.launch +++ b/code/planning/global_planner/launch/global_planner.launch @@ -1,7 +1,7 @@ diff --git a/code/planning/local_planner/package.xml b/code/planning/local_planner/package.xml index 6dc4269a..a1e4978c 100644 --- a/code/planning/local_planner/package.xml +++ b/code/planning/local_planner/package.xml @@ -32,7 +32,6 @@ - @@ -48,12 +47,14 @@ + perception + perception + perception catkin - diff --git a/code/planning/local_planner/src/ACC.py b/code/planning/local_planner/src/ACC.py new file mode 100755 index 00000000..d13357fa --- /dev/null +++ b/code/planning/local_planner/src/ACC.py @@ -0,0 +1,207 @@ +#!/usr/bin/env python +import ros_compatibility as roscomp +# import tf.transformations +from ros_compatibility.node import CompatibleNode +from rospy import Subscriber, Publisher +from geometry_msgs.msg import PoseStamped +from carla_msgs.msg import CarlaSpeedometer # , CarlaWorldInfo +from nav_msgs.msg import Path +# from std_msgs.msg import String +from std_msgs.msg import Float32MultiArray, Float32 +from collision_check import CollisionCheck +import time +from perception.msg import MinDistance + + +class ACC(CompatibleNode): + """ + This node recieves a possible collision and + """ + + def __init__(self): + super(ACC, self).__init__('ACC') + self.role_name = self.get_param("role_name", "hero") + self.control_loop_rate = self.get_param("control_loop_rate", 1) + self.current_speed = 50 / 3.6 # m/ss + + # Get current speed + self.velocity_sub: Subscriber = self.new_subscription( + CarlaSpeedometer, + f"/carla/{self.role_name}/Speed", + self.__get_current_velocity, + qos_profile=1) + + # Subscriber for lidar distance + # TODO: Change to real lidar distance + self.lidar_dist = self.new_subscription( + MinDistance, + f"/paf/{self.role_name}/Center/min_distance", + self._set_distance, + qos_profile=1) + # Get initial set of speed limits + self.speed_limit_OD_sub: Subscriber = self.new_subscription( + Float32MultiArray, + f"/paf/{self.role_name}/speed_limits_OpenDrive", + self.__set_speed_limits_opendrive, + qos_profile=1) + + # Get trajectory to determine current speed limit + self.trajectory_sub: Subscriber = self.new_subscription( + Path, + f"/paf/{self.role_name}/trajectory", + self.__set_trajectory, + qos_profile=1) + + self.emergency_sub: Subscriber = self.new_subscription( + msg_type=PoseStamped, + topic="/paf/" + self.role_name + "/current_pos", + callback=self.__current_position_callback, + qos_profile=1) + self.approx_speed_sub = self.new_subscription( + Float32, + f"/paf/{self.role_name}/cc_speed", + self.__approx_speed_callback, + qos_profile=1) + # Publish desired speed to acting + self.velocity_pub: Publisher = self.new_publisher( + Float32, + f"/paf/{self.role_name}/acc_velocity", + qos_profile=1) + + # List of all speed limits, sorted by waypoint index + self.__speed_limits_OD: [float] = [] + # Current Trajectory + self.__trajectory: Path = None + # Current index from waypoint + self.__current_wp_index: int = 0 + # Current speed + self.__current_velocity: float = None + # Distance and speed from possible collsion object + self.obstacle_speed: tuple = None + # Obstalce distance + self.obstacle_distance = None + # Current speed limit + self.speed_limit: float = None # m/s + + def _set_distance(self, data: MinDistance): + """Get min distance to object in front from perception + + Args: + data (MinDistance): Minimum Distance from LIDAR + """ + self.obstacle_distance = data.distance + + def __approx_speed_callback(self, data: Float32): + """Safe approximated speed form obstacle in front together with + timestamp when recieved. + Timestamp is needed to check wether we still have a vehicle in front + + Args: + data (Float32): Speed from obstacle in front + """ + self.obstacle_speed = (time.time(), data.data) + + def __get_current_velocity(self, data: CarlaSpeedometer): + """_summary_ + + Args: + data (CarlaSpeedometer): _description_ + """ + self.__current_velocity = float(data.speed) + + def __set_trajectory(self, data: Path): + """Recieve trajectory from global planner + + Args: + data (Path): Trajectory path + """ + self.__trajectory = data + + def __set_speed_limits_opendrive(self, data: Float32MultiArray): + """Recieve speed limits from OpenDrive via global planner + + Args: + data (Float32MultiArray): speed limits per waypoint + """ + self.__speed_limits_OD = data.data + + def __current_position_callback(self, data: PoseStamped): + """Get current position and check if next waypoint is reached + If yes -> update current waypoint and speed limit + + Args: + data (PoseStamped): Current position from perception + """ + if len(self.__speed_limits_OD) < 1 or self.__trajectory is None: + return + + agent = data.pose.position + current_wp = self.__trajectory.poses[self.__current_wp_index].\ + pose.position + next_wp = self.__trajectory.poses[self.__current_wp_index + 1].\ + pose.position + # distances from agent to current and next waypoint + d_old = abs(agent.x - current_wp.x) + abs(agent.y - current_wp.y) + d_new = abs(agent.x - next_wp.x) + abs(agent.y - next_wp.y) + if d_new < d_old: + # update current waypoint and corresponding speed limit + self.__current_wp_index += 1 + self.speed_limit = \ + self.__speed_limits_OD[self.__current_wp_index] + + def run(self): + """ + Control loop + :return: + """ + def loop(timer_event=None): + """ + Checks if distance to a possible object is too small and + publishes the desired speed to motion planning + """ + if self.obstacle_speed is not None: + # Check if too much time has passed since last speed update + if self.obstacle_speed[0] + 0.5 < time.time(): + self.obstacle_speed = None + + if self.obstacle_distance is not None and \ + self.obstacle_speed is not None and \ + self.__current_velocity is not None: + # If we have obstalce speed and distance, we can + # calculate the safe speed + safety_distance = CollisionCheck.calculate_rule_of_thumb( + False, self.__current_velocity) + if self.obstacle_distance < safety_distance: + # If safety distance is reached, we want to reduce the + # speed to meet the desired distance + safe_speed = self.obstacle_speed[1] * \ + (self.obstacle_distance / safety_distance) + self.velocity_pub.publish(safe_speed) + else: + # If safety distance is reached, drive with same speed as + # Object in front + self.velocity_pub.publish(self.obstacle_speed[1]) + + elif self.speed_limit is not None: + # If we have no obstacle, we want to drive with the current + # speed limit + self.velocity_pub.publish(self.speed_limit) + + self.new_timer(self.control_loop_rate, loop) + self.spin() + + +if __name__ == "__main__": + """ + main function starts the ACC node + :param args: + """ + roscomp.init('ACC') + + try: + node = ACC() + node.run() + except KeyboardInterrupt: + pass + finally: + roscomp.shutdown() diff --git a/code/planning/local_planner/src/collision_check.py b/code/planning/local_planner/src/collision_check.py index 34bcdbb4..5fe0d95a 100755 --- a/code/planning/local_planner/src/collision_check.py +++ b/code/planning/local_planner/src/collision_check.py @@ -1,14 +1,16 @@ #!/usr/bin/env python # import rospy +import numpy as np # import tf.transformations import ros_compatibility as roscomp from ros_compatibility.node import CompatibleNode - -# from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion -# from carla_msgs.msg import CarlaRoute # , CarlaWorldInfo -# from nav_msgs.msg import Path +from rospy import Subscriber +from carla_msgs.msg import CarlaSpeedometer # , CarlaWorldInfo # from std_msgs.msg import String -# from std_msgs.msg import Float32MultiArray +from std_msgs.msg import Float32, Float32MultiArray +from std_msgs.msg import Bool +from perception.msg import MinDistance +import time class CollisionCheck(CompatibleNode): @@ -21,73 +23,173 @@ def __init__(self): super(CollisionCheck, self).__init__('CollisionCheck') self.role_name = self.get_param("role_name", "hero") self.control_loop_rate = self.get_param("control_loop_rate", 1) - self.current_speed = 50 / 3.6 # m/ss + # self.current_speed = 50 / 3.6 # m/ss # TODO: Add Subscriber for Speed and Obstacles - self.logdebug("CollisionCheck started") - - def update(self, speed): - self.current_speed = speed + self.loginfo("CollisionCheck started") + + # self.obstacle_sub: Subscriber = self.new_subscription( + # ) + # Subscriber for current speed + self.velocity_sub: Subscriber = self.new_subscription( + CarlaSpeedometer, + f"/carla/{self.role_name}/Speed", + self.__get_current_velocity, + qos_profile=1) + # Subscriber for lidar distance + # TODO: Change to real lidar distance + self.lidar_dist = self.new_subscription( + MinDistance, + f"/paf/{self.role_name}/Center/min_distance", + self.calculate_obstacle_speed, + qos_profile=1) + # Publisher for emergency stop + self.emergency_pub = self.new_publisher( + Bool, + f"/paf/{self.role_name}/emergency", + qos_profile=1) + # Publisher for distance to collision + self.collision_pub = self.new_publisher( + Float32MultiArray, + f"/paf/{self.role_name}/collision", + qos_profile=1) + # Approx speed publisher for ACC + self.speed_publisher = self.new_publisher( + Float32, + f"/paf/{self.role_name}/cc_speed", + qos_profile=1) + # Variables to save vehicle data + self.__current_velocity: float = None + self.__object_last_position: tuple = None + + def calculate_obstacle_speed(self, new_dist: MinDistance): + """Caluclate the speed of the obstacle in front of the ego vehicle + based on the distance between to timestamps + + Args: + new_position (MinDistance): new position received from the lidar + """ + # Check if current speed from vehicle is not None + if self.__current_velocity is None: + return + # Check if this is the first time the callback is called + if self.__object_last_position is None and \ + np.isinf(new_dist.distance) is not True: + self.__object_last_position = (time.time(), + new_dist.distance) + return + + # If distance is np.inf no car is in front + if np.isinf(new_dist.distance): + self.__object_last_position = None + return + # Check if too much time has passed since last position update + if self.__object_last_position[0] + \ + 0.5 < time.time(): + self.__object_last_position = (time.time(), + new_dist.distance) + return + # Calculate time since last position update + current_time = time.time() + time_difference = current_time-self.__object_last_position[0] + + # Calculate distance (in m) + distance = new_dist.distance - self.__object_last_position[1] + + # Speed is distance/time (m/s) + relative_speed = distance/time_difference + speed = self.__current_velocity + relative_speed + # Publish speed to ACC for permanent distance check + self.speed_publisher.publish(Float32(data=speed)) + # Check for crash + self.check_crash((new_dist.distance, speed)) + self.__object_last_position = (current_time, new_dist.distance) + + def __get_current_velocity(self, data: CarlaSpeedometer,): + """Saves current velocity of the ego vehicle + + Args: + data (CarlaSpeedometer): Message from carla with current speed + """ + self.__current_velocity = float(data.speed) def time_to_collision(self, obstacle_speed, distance): - return distance / (self.current_speed - obstacle_speed) + """calculates the time to collision with the obstacle in front + + Args: + obstacle_speed (float): Speed from obstacle in front + distance (float): Distance to obstacle in front + + Returns: + float: Time until collision with obstacle in front + """ + if (self.__current_velocity - obstacle_speed) == 0: + return -1 + return distance / (self.__current_velocity - obstacle_speed) def meters_to_collision(self, obstacle_speed, distance): - return self.time_to_collision(obstacle_speed, distance) * \ - self.current_speed + """Calculates the meters until collision with the obstacle in front - # PAF 22 - def calculate_safe_dist(self) -> float: + Args: + obstacle_speed (float): speed from obstacle in front + distance (float): distance from obstacle in front + + Returns: + float: distance (in meters) until collision with obstacle in front """ - Calculates the distance you have to keep to the vehicle in front to - have t_reaction to react to the vehicle suddenly stopping - The formula replicates official recommendations for safe distances + return self.time_to_collision(obstacle_speed, distance) * \ + self.__current_velocity + + @staticmethod + def calculate_rule_of_thumb(emergency, speed): + """Calculates the rule of thumb as approximation + for the braking distance + + Args: + emergency (bool): if emergency brake is initiated + + Returns: + float: distance calculated with rule of thumb """ - t_reaction = 1 # s - t_breaking = 1 # s - a = 8 # m/s^2 - v = self.current_speed - s = - 0.5 * a * t_breaking ** 2 + v * t_breaking + v * t_reaction - return s + 5 - - def calculate_rule_of_thumb(self, emergency): - reaction_distance = self.current_speed - braking_distance = (self.current_speed * 0.36)**2 + reaction_distance = speed + braking_distance = (speed * 0.36)**2 if emergency: return reaction_distance + braking_distance / 2 else: return reaction_distance + braking_distance def check_crash(self, obstacle): + """ Checks if and when the ego vehicle will crash + with the obstacle in front + + Args: + obstacle (tuple): tuple with distance and + speed from obstacle in front + """ distance, obstacle_speed = obstacle collision_time = self.time_to_collision(obstacle_speed, distance) - collision_meter = self.meters_to_collision(obstacle_speed, distance) - - safe_distance = self.calculate_safe_dist() - safe_distance2 = self.calculate_rule_of_thumb(False) - emergency_distance2 = self.calculate_rule_of_thumb(True) - - # TODO: Convert to Publishers + # collision_meter = self.meters_to_collision(obstacle_speed, distance) + # safe_distance2 = self.calculate_rule_of_thumb(False) + emergency_distance2 = self.calculate_rule_of_thumb( + True, self.__current_velocity) if collision_time > 0: if distance < emergency_distance2: - print(f"Emergency Brake needed, {emergency_distance2:.2f}") - print(f"Ego reaches obstacle after {collision_time:.2f} seconds.") - print(f"Ego reaches obstacle after {collision_meter:.2f} meters.") - print(f"Safe Distance PAF 22: {safe_distance:.2f}") - print(f"Safe Distance Thumb: {safe_distance2:.2f}") + # Initiate emergency brake + self.emergency_pub.publish(True) + return + # When no emergency brake is needed publish collision object + data = Float32MultiArray(data=[distance, obstacle_speed]) + self.collision_pub.publish(data) else: - print("Ego slower then car in front") + # If no collision is ahead publish np.Inf + data = Float32MultiArray(data=[np.Inf, obstacle_speed]) + self.collision_pub.publish(data) def run(self): """ Control loop :return: """ - - def loop(timer_event=None): - pass - - self.new_timer(self.control_loop_rate, loop) self.spin() diff --git a/code/planning/local_planner/src/dev_collision_publisher.py b/code/planning/local_planner/src/dev_collision_publisher.py new file mode 100755 index 00000000..9bbd267a --- /dev/null +++ b/code/planning/local_planner/src/dev_collision_publisher.py @@ -0,0 +1,103 @@ +#!/usr/bin/env python +# import rospy +# import tf.transformations +import ros_compatibility as roscomp +from ros_compatibility.node import CompatibleNode + +# from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion +# from carla_msgs.msg import CarlaRoute # , CarlaWorldInfo +# from nav_msgs.msg import Path +# from std_msgs.msg import String +from std_msgs.msg import Float32 +from carla_msgs.msg import CarlaSpeedometer +import time +from perception.msg import MinDistance +# import numpy as np + + +class DevCollisionCheck(CompatibleNode): + """ + This is currently a test node. In the future this node will be + responsible for detecting collisions and reporting them. + """ + + def __init__(self): + super(DevCollisionCheck, self).__init__('DevCollisionCheck') + self.role_name = self.get_param("role_name", "hero") + self.control_loop_rate = self.get_param("control_loop_rate", 1) + + self.pub_lidar = self.new_publisher( + msg_type=MinDistance, + topic=f'/paf/{self.role_name}/Center/min_distance', + qos_profile=1) + + self.pub_test_speed = self.new_publisher( + msg_type=CarlaSpeedometer, + topic='/carla/' + self.role_name + '/test_speed', + qos_profile=1) + self.sub_ACC = self.new_subscription( + msg_type=Float32, + topic='/paf/' + self.role_name + '/acc_velocity', + callback=self.callback_ACC, + qos_profile=1) + + self.sub_manual = self.new_subscription( + msg_type=Float32, + topic='/paf/' + self.role_name + '/manual', + callback=self.callback_manual, + qos_profile=1) + self.loginfo("DevCollisionCheck started") + self.last_position_update = None + self.simulated_speed = 12 # m/s + self.distance_to_collision = 0 + self.current_speed = 0 + self.manual_start = True + self.acc_activated = False + + def callback_manual(self, msg: Float32): + if self.manual_start: + self.manual_start = False + self.pub_lidar.publish(MinDistance(distance=25)) + time.sleep(0.2) + self.pub_lidar.publish(MinDistance(distance=25)) + time.sleep(0.2) + self.pub_lidar.publish(MinDistance(distance=24)) + # time.sleep(0.2) + # self.pub_lidar.publish(Float32(data=20)) + # time.sleep(0.2) + # self.pub_lidar.publish(Float32(data=20)) + # time.sleep(0.2) + # self.pub_lidar.publish(Float32(data=20)) + + def callback_ACC(self, msg: Float32): + self.acc_activated = True + self.logerr("Timestamp: " + time.time().__str__()) + self.logerr("ACC: " + str(msg.data)) + self.current_speed = msg.data + + def run(self): + """ + Control loop + :return: + """ + def loop(timer_event=None): + self.pub_test_speed.publish(CarlaSpeedometer(speed=13.8889)) + + self.new_timer(self.control_loop_rate, loop) + self.spin() + + +if __name__ == "__main__": + """ + main function starts the CollisionCheck node + :param args: + """ + roscomp.init('DevCollisionCheck') + + try: + node = DevCollisionCheck() + node.run() + except KeyboardInterrupt: + pass + finally: + roscomp.shutdown() diff --git a/doc/03_research/Leaderboard-2/capture_sensor_data.py b/doc/03_research/Leaderboard-2/capture_sensor_data.py deleted file mode 100644 index ee2b26df..00000000 --- a/doc/03_research/Leaderboard-2/capture_sensor_data.py +++ /dev/null @@ -1,431 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma de -# Barcelona (UAB). -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . - -""" -Welcome to the capture sensor data script, a script that provides users with a baseline for data collection, -which they can later modify to their specific needs, easying the process of creating a database. - -This script will start with a CARLA recorder log, spawning the desired sensor configuration at the ego vehicle, -and saving their data into a folder. The exact parameters can be found at the very top of the script and include: - -> SENSORS: List of all the sensors tha will be spawned in the simulation -> WEATHER: Weather of the simulation -> RECORDER_INFO: List of all the CARLA recorder logs that will be run. Each recorder has three elements: - - folder: path to the folder with the `*.log` recorder file, and a `log.json` file, which has all the ego vehicle information - - start_time: start time of the recorder - - duration: duration of the recorder -""" - -import time -import os -import carla -import argparse -import random -import json -import threading -import glob - -from queue import Queue, Empty - -################### User simulation configuration #################### -# 1) Choose the sensors -SENSORS = [ - [ - 'CameraTest', - { - 'bp': 'sensor.camera.rgb', - 'image_size_x': 720, 'image_size_y': 1080, 'fov': 100, - 'x': 0.7, 'y': 0.0, 'z': 1.60, 'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0 - }, - ], - [ - 'LidarTest', - { - 'bp': 'sensor.lidar.ray_cast', - 'x': 0.7, 'y': 0.0, 'z': 1.60, 'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0 - } - ], - [ - 'SemanticLidarTest', - { - 'bp': 'sensor.lidar.ray_cast_semantic', - 'x': 0.7, 'y': 0.0, 'z': 1.60, 'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0 - } - ], - [ - 'RADARTest', - { - 'bp': 'sensor.other.radar', - 'x': 0.7, 'y': 0.0, 'z': 1.60, 'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0 - } - ], - [ - 'GnssTest', - { - 'bp': 'sensor.other.gnss', - 'x': 0.7, 'y': 0.0, 'z': 1.60, 'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0 - } - ], - [ - 'IMUTest', - { - 'bp': 'sensor.other.imu', - 'x': 0.7, 'y': 0.0, 'z': 1.60, 'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0 - } - ] -] - -# 2) Choose a weather -WEATHER = carla.WeatherParameters( - sun_azimuth_angle=-1.0, sun_altitude_angle=70.0, - cloudiness=30.0, precipitation=0.0, precipitation_deposits=80.0, wetness=15.0, - wind_intensity=10.0, - fog_density=2.0, fog_distance=0.0, fog_falloff=0.0) - -# 3) Choose a recorder file (folder path, start time, duration) -RECORDER_INFO = [ - { - 'folder': "records/Town10_1", - 'start_time': 20, - 'duration': 10 - }, - { - 'folder': "records/Town10_1", - 'start_time': 80, - 'duration': 5 - } -] - -# 4) Choose the destination folder where the sensor data will be saved -DESTINATION_FOLDER = "database" -################# End user simulation configuration ################## - -FPS = 20 -THREADS = 5 -CURRENT_THREADS = 0 - -def create_folders(endpoint, sensors): - for sensor_id, sensor_bp in sensors: - sensor_endpoint = f"{endpoint}/{sensor_id}" - if not os.path.exists(sensor_endpoint): - os.makedirs(sensor_endpoint) - - if 'gnss' in sensor_bp: - sensor_endpoint = f"{endpoint}/{sensor_id}/gnss_data.csv" - with open(sensor_endpoint, 'w') as data_file: - data_txt = f"Frame,Altitude,Latitude,Longitude\n" - data_file.write(data_txt) - - if 'imu' in sensor_bp: - sensor_endpoint = f"{endpoint}/{sensor_id}/imu_data.csv" - with open(sensor_endpoint, 'w') as data_file: - data_txt = f"Frame,Accelerometer X,Accelerometer y,Accelerometer Z,Compass,Gyroscope X,Gyroscope Y,Gyroscope Z\n" - data_file.write(data_txt) - -def add_listener(sensor, sensor_queue, sensor_id): - sensor.listen(lambda data: sensor_listen(data, sensor_queue, sensor_id)) - -def sensor_listen(data, sensor_queue, sensor_id): - sensor_queue.put((sensor_id, data.frame, data)) - return - -def get_ego_id(recorder_file): - found_lincoln = False - found_id = None - - for line in recorder_file.split("\n"): - - # Check the role_name for hero - if found_lincoln: - if not line.startswith(" "): - found_lincoln = False - found_id = None - else: - data = line.split(" = ") - if 'role_name' in data[0] and 'hero' in data[1]: - return found_id - - # Search for all lincoln vehicles - if not found_lincoln and line.startswith(" Create ") and 'vehicle.lincoln' in line: - found_lincoln = True - found_id = int(line.split(" ")[2][:-1]) - - return found_id - -def save_data_to_disk(sensor_id, frame, data, imu_data, endpoint): - """ - Saves the sensor data into file: - - Images -> '.png', one per frame, named as the frame id - - Lidar: -> '.ply', one per frame, named as the frame id - - SemanticLidar: -> '.ply', one per frame, named as the frame id - - RADAR: -> '.csv', one per frame, named as the frame id - - GNSS: -> '.csv', one line per frame, named 'gnss_data.csv' - - IMU: -> '.csv', one line per frame, named 'imu_data.csv' - """ - global CURRENT_THREADS - CURRENT_THREADS += 1 - if isinstance(data, carla.Image): - sensor_endpoint = f"{endpoint}/{sensor_id}/{frame}.png" - data.save_to_disk(sensor_endpoint) - - elif isinstance(data, carla.LidarMeasurement): - sensor_endpoint = f"{endpoint}/{sensor_id}/{frame}.ply" - data.save_to_disk(sensor_endpoint) - - elif isinstance(data, carla.SemanticLidarMeasurement): - sensor_endpoint = f"{endpoint}/{sensor_id}/{frame}.ply" - data.save_to_disk(sensor_endpoint) - - elif isinstance(data, carla.RadarMeasurement): - sensor_endpoint = f"{endpoint}/{sensor_id}/{frame}.csv" - data_txt = f"Altitude,Azimuth,Depth,Velocity\n" - for point_data in data: - data_txt += f"{point_data.altitude},{point_data.azimuth},{point_data.depth},{point_data.velocity}\n" - with open(sensor_endpoint, 'w') as data_file: - data_file.write(data_txt) - - elif isinstance(data, carla.GnssMeasurement): - sensor_endpoint = f"{endpoint}/{sensor_id}/gnss_data.csv" - with open(sensor_endpoint, 'a') as data_file: - data_txt = f"{frame},{data.altitude},{data.latitude},{data.longitude}\n" - data_file.write(data_txt) - - elif isinstance(data, carla.IMUMeasurement): - sensor_endpoint = f"{endpoint}/{sensor_id}/imu_data.csv" - with open(sensor_endpoint, 'a') as data_file: - data_txt = f"{frame},{imu_data[0][0]},{imu_data[0][1]},{imu_data[0][2]},{data.compass},{imu_data[1][0]},{imu_data[1][1]},{imu_data[1][2]}\n" - data_file.write(data_txt) - - else: - print(f"WARNING: Ignoring sensor '{sensor_id}', as no callback method is known for data of type '{type(data)}'.") - - CURRENT_THREADS -= 1 - -def extract_imu_data(log): - records = log["records"] - log_data = [] - for record in records: - acceleration_data = record["state"]["acceleration"] - acceleration_vector = [acceleration_data["x"], acceleration_data["y"], acceleration_data["z"]] - - # TODO: Remove this (don't use logs without angular velocity) - if "angular_velcoity" in record["state"]: - angular_data = record["state"]["angular_velocity"] - angular_vector = [angular_data["x"], angular_data["y"], angular_data["z"]] - else: - angular_vector = [random.random(), random.random(), random.random()] - - log_data.append([acceleration_vector, angular_vector]) - - return log_data - -def main(): - - argparser = argparse.ArgumentParser(description=__doc__) - argparser.add_argument('--host', default='127.0.0.1', help='IP of the host server (default: 127.0.0.1)') - argparser.add_argument('--port', default=2000, type=int, help='TCP port to listen to (default: 2000)') - args = argparser.parse_args() - print(__doc__) - - active_sensors = [] - - try: - - # Initialize the simulation - client = carla.Client(args.host, args.port) - client.set_timeout(120.0) - world = client.get_world() - - for recorder_info in RECORDER_INFO: - recorder_folder = recorder_info['folder'] - recorder_start = recorder_info['start_time'] - recorder_duration = recorder_info['duration'] - - # 0) Get the recorder files - recorder_path_list = glob.glob(f"{os.getcwd()}/{recorder_folder}/*.log") - if recorder_path_list: - recorder_path = recorder_path_list[0] - else: - print(f"Couldn't find the recorder file for the folder '{recorder_folder}'. Stopping...") - continue - recorder_log_list = glob.glob(f"{os.getcwd()}/{recorder_folder}/log.json") - if recorder_log_list: - recorder_log = recorder_log_list[0] - else: - recorder_log = None - - print(f"Running: {recorder_path}") - endpoint = f"{DESTINATION_FOLDER}/{recorder_path.split('/')[-1][:-4]}" - - # 1) Get the recorder map and load the world - recorder_str = client.show_recorder_file_info(recorder_path, True) - recorder_map = recorder_str.split("\n")[1][5:] - world = client.load_world(recorder_map) - world.tick() - - # 2) Change the weather and synchronous mode - world.set_weather(WEATHER) - settings = world.get_settings() - settings.fixed_delta_seconds = 1 / FPS - settings.synchronous_mode = True - world.apply_settings(settings) - - for _ in range(100): - world.tick() - - # 3) Replay the recorder - max_duration = float(recorder_str.split("\n")[-2].split(" ")[1]) - if recorder_duration == 0: - recorder_duration = max_duration - elif recorder_start + recorder_duration > max_duration: - print("WARNING: Found a duration that exceeds the recorder length. Reducing it...") - recorder_duration = max_duration - recorder_start - if recorder_start >= max_duration: - print("WARNING: Found a start point that exceeds the recoder duration. Ignoring it...") - continue - print(f"Duration: {round(recorder_duration, 2)} - Frames: {round(20*recorder_duration, 0)}") - - if recorder_log: - with open(recorder_log) as fd: - log_json = json.load(fd) - imu_logs = extract_imu_data(log_json) - else: - imu_logs = None - - client.replay_file(recorder_path, recorder_start, recorder_duration, get_ego_id(recorder_str), False) - with open(f"{recorder_path[:-4]}.txt", 'w') as fd: - fd.write(recorder_str) - world.tick() - - # 4) Link onto the ego vehicle - hero = None - while hero is None: - print("Waiting for the ego vehicle...") - possible_vehicles = world.get_actors().filter('vehicle.*') - for vehicle in possible_vehicles: - if vehicle.attributes['role_name'] == 'hero': - print("Ego vehicle found") - hero = vehicle - break - time.sleep(5) - - # 5) Create the sensors, and save their data into a queue - create_folders(endpoint, [[s[0], s[1].get('bp')] for s in SENSORS]) - blueprint_library = world.get_blueprint_library() - sensor_queue = Queue() - for sensor in SENSORS: - - # Extract the data from the sesor configuration - sensor_id = sensor[0] - attributes = sensor[1] - blueprint_name = attributes.get('bp') - sensor_transform = carla.Transform( - carla.Location(x=attributes.get('x'), y=attributes.get('y'), z=attributes.get('z')), - carla.Rotation(pitch=attributes.get('pitch'), roll=attributes.get('roll'), yaw=attributes.get('yaw')) - ) - - # Get the blueprint and add the attributes - blueprint = blueprint_library.find(blueprint_name) - for key, value in attributes.items(): - if key in ['bp', 'x', 'y', 'z', 'roll', 'pitch', 'yaw']: - continue - blueprint.set_attribute(str(key), str(value)) - - # Create the sensors and its callback - sensor = world.spawn_actor(blueprint, sensor_transform, hero) - add_listener(sensor, sensor_queue, sensor_id) - active_sensors.append(sensor) - - world.tick() - - # 6) Run the simulation - start_time = world.get_snapshot().timestamp.elapsed_seconds - start_frame = world.get_snapshot().frame - sensor_amount = len(SENSORS) - - max_threads = THREADS - results = [] - - while True: - current_time = world.get_snapshot().timestamp.elapsed_seconds - current_duration = current_time - start_time - if current_duration >= recorder_duration: - print(f">>>>> Running recorded simulation: 100.00% completed <<<<<") - break - - completion = format(round(current_duration / recorder_duration * 100, 2), '3.2f') - print(f">>>>> Running recorded simulation: {completion}% completed <<<<<", end="\r") - - # Get and save the sensor data from the queue. - missing_sensors = sensor_amount - while True: - - frame = world.get_snapshot().frame - try: - sensor_data = sensor_queue.get(True, 2.0) - if sensor_data[1] != frame: - continue # Ignore previous frame data - missing_sensors -= 1 - except Empty: - raise ValueError("A sensor took too long to send their data") - - # Get the data - sensor_id = sensor_data[0] - frame_diff = sensor_data[1] - start_frame - data = sensor_data[2] - if imu_logs: - imu_data = imu_logs[int(FPS*recorder_start + frame_diff)] - else: - imu_data = [[0,0,0], [0,0,0]] - - res = threading.Thread(target=save_data_to_disk, args=(sensor_id, frame_diff, data, imu_data, endpoint)) - results.append(res) - res.start() - - if CURRENT_THREADS > max_threads: - for res in results: - res.join() - results = [] - - if missing_sensors <= 0: - break - - world.tick() - - for res in results: - res.join() - - for sensor in active_sensors: - sensor.stop() - sensor.destroy() - active_sensors = [] - - for _ in range(50): - world.tick() - - # End the simulation - finally: - # stop and remove cameras - for sensor in active_sensors: - sensor.stop() - sensor.destroy() - - # set fixed time step length - settings = world.get_settings() - settings.fixed_delta_seconds = None - settings.synchronous_mode = False - world.apply_settings(settings) - -if __name__ == '__main__': - - try: - main() - except KeyboardInterrupt: - pass - finally: - print('\ndone.') diff --git a/doc/07_planning/local_planning.md b/doc/07_planning/local_planning.md new file mode 100644 index 00000000..cefa0a5e --- /dev/null +++ b/doc/07_planning/local_planning.md @@ -0,0 +1,104 @@ +# Local Planning + +**Summary:** Documentation of Collision Check node and ACC in package local planner + +--- + +## Author + +Samuel Kühnel + +## Date + +17.12.2023 + +## Collsision Check + +### Subscibed Topics + +* `/carla/hero/Speed`: Get current vehicle speed +* `/paf/hero/Center/min_distance`: Get min distance from LIDAR + +### Published Topics+ + +* `/paf/hero/emergency`: Boolean that indicates if emergency brake is needed +* `/paf/hero/collision`: Collision object (Float32 Array) with distance and speed of obstacle in front + +### Tasks + +#### Calculate speed of obstacle in front + +When the node recieves a distance from the LIDAR it is saved together with a timestamp so when the next distance message arrives the speed can be approximated. + +This could be removed in the future, as when the radar gets involved the speed no longer needs to be approximated. + +#### Check if crash is ahead + +The Collision Check checks based on the current speed and last distance if a collsision with the obstacle in front is ahead. + +The code looks like this: + +```python + obstacle_speed, distance = obstacle + collision_time = time_to_collision(obstacle_speed, distance) + # Calculation of distance for emergency stop + emergency_distance = calculate_rule_of_thumb(True, __current_velocity) + if collision_time > 0: + # Check if emergency brake is needed to stop + if distance < emergency_distance: + # Initiate emergency brake + self.emergency_pub.publish(True) + return + # When no emergency brake is needed publish collision distance for + # ACC and Behaviour tree + data = Float32MultiArray(data=[distance, obstacle_speed]) + self.collision_pub.publish(data) + else: + # If no collision is ahead publish np.Inf + data = Float32MultiArray(data=[np.Inf, obstacle_speed]) +``` + +For calculating the distance the "rule of thumb" is used. + +$$ + distance_{safety} = speed + (speed \cdot 0.36)^2 +$$ + +$$ + distance_{emergency} = speed + \frac{(speed \cdot 0.36)^2}{2} +$$ + +## ACC + +### Subscibed Topics + +* `/carla/hero/Speed`: Get current vehicle speed +* `/paf/hero/collision`: Get the collision object +* `/paf/hero/speed_limits_OpenDrive`: Get speedlimits from waypoints +* `/paf/hero/trajectory`: Get current trajectory +* `/paf/hero/emergency`: Deactivate ACC if emergency brake is initiated +* `/paf/hero/current_pos`: Get current position + +### Published Topics+ + +* `/paf/hero/acc_velocity`: Velocity to keep distance to object in front + +### Tasks + +#### Get current speed limit + +The ACC subscribes to the trajectory, speed limit and current position. + +Every time the current position is updated the node calculates the current speed limit based on the trajectory and the speedlimits ordered by the waypoints. + +#### Calculate speed for motion planning + +By default the node publishes the current speed limit. + +If a collision is recieved by the Collision Check the loop gets deactivated and a appropriate speed is calculated by this formula: + +$$ +speed_{acc} = speed_{obstacle} \cdot \frac{distance_{obstacle}}{distance_{safety}} +$$ + +The ACC speed depends on the obstacles speed and distance.