From 9f28ba3c3fda6b6b354c71331bfe622f41b6d767 Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Mon, 18 Mar 2024 17:16:28 +0100 Subject: [PATCH 1/2] fix: tune Planning for new Carla fork; Remove FOT from build --- build/docker-compose.yml | 2 +- build/docker/agent/Dockerfile | 13 ---- code/acting/launch/acting.launch | 4 +- .../src/behavior_agent/behaviours/overtake.py | 8 ++- .../behaviours/road_features.py | 2 +- code/planning/src/local_planner/ACC.py | 12 ++-- .../src/local_planner/collision_check.py | 13 ++-- .../src/local_planner/motion_planning.py | 8 +-- code/planning/src/local_planner/utils.py | 60 +++++++------------ 9 files changed, 49 insertions(+), 73 deletions(-) diff --git a/build/docker-compose.yml b/build/docker-compose.yml index 1372dfdb..6c7ffe47 100644 --- a/build/docker-compose.yml +++ b/build/docker-compose.yml @@ -21,7 +21,7 @@ services: # based on https://github.com/ll7/paf21-1/blob/master/scenarios/docker-carla-sim-compose.yml carla-simulator: - command: /bin/bash CarlaUE4.sh -quality-level=Epic -world-port=2000 -resx=800 -resy=600 -nosound -carla-settings="/home/carla/CarlaUE4/Config/CustomCarlaSettings.ini" + command: /bin/bash CarlaUE4.sh -quality-level=Low -world-port=2000 -resx=800 -resy=600 -nosound -carla-settings="/home/carla/CarlaUE4/Config/CustomCarlaSettings.ini" image: ghcr.io/una-auxme/paf23:leaderboard-2.0 init: true deploy: diff --git a/build/docker/agent/Dockerfile b/build/docker/agent/Dockerfile index 93b677b3..bee277af 100644 --- a/build/docker/agent/Dockerfile +++ b/build/docker/agent/Dockerfile @@ -162,19 +162,6 @@ RUN source ~/.bashrc && pip install -r /workspace/requirements.txt # Add agent code COPY --chown=$USERNAME:$USERNAME ./code /workspace/code/ -# Install Frenet Optimal Trajectory Planner -RUN sudo mkdir -p /erdos/dependencies/frenet_optimal_trajectory_planner -# Needed to resolve dependencies correctly inside freent_optimal_trajectory_planner -ENV PYLOT_HOME=/erdos - -ENV FREENET_HOME=/erdos/dependencies/frenet_optimal_trajectory_planner -RUN sudo chown $USERNAME:$USERNAME $PYLOT_HOME -RUN sudo chown $USERNAME:$USERNAME $FREENET_HOME - -RUN git clone https://github.com/erdos-project/frenet_optimal_trajectory_planner.git $FREENET_HOME -RUN cd $FREENET_HOME && source ./build.sh - -ENV PYTHONPATH=$PYTHONPATH:/erdos/dependencies # Link code into catkin workspace RUN ln -s /workspace/code /catkin_ws/src diff --git a/code/acting/launch/acting.launch b/code/acting/launch/acting.launch index ba12415c..19f9a88d 100644 --- a/code/acting/launch/acting.launch +++ b/code/acting/launch/acting.launch @@ -14,12 +14,12 @@ - + - + diff --git a/code/planning/src/behavior_agent/behaviours/overtake.py b/code/planning/src/behavior_agent/behaviours/overtake.py index bf0941be..08d6d5b1 100644 --- a/code/planning/src/behavior_agent/behaviours/overtake.py +++ b/code/planning/src/behavior_agent/behaviours/overtake.py @@ -5,6 +5,8 @@ import numpy as np from . import behavior_speed as bs +import planning # noqa: F401 +from local_planner.utils import NUM_WAYPOINTS """ Source: https://github.com/ll7/psaf2 @@ -15,8 +17,8 @@ def convert_to_ms(speed): return speed / 3.6 -# Varaible to determine if overtake is currently exec -OVERTAKE_EXECUTING = False +# Varaible to determine the distance to overtak the object +OVERTAKE_EXECUTING = 0 class Approach(py_trees.behaviour.Behaviour): @@ -413,7 +415,7 @@ def update(self): self.current_pos = np.array([data.pose.position.x, data.pose.position.y]) distance = np.linalg.norm(self.first_pos - self.current_pos) - if distance > OVERTAKE_EXECUTING: + if distance > OVERTAKE_EXECUTING + NUM_WAYPOINTS: rospy.loginfo(f"Left Overtake: {self.current_pos}") return py_trees.common.Status.FAILURE else: diff --git a/code/planning/src/behavior_agent/behaviours/road_features.py b/code/planning/src/behavior_agent/behaviours/road_features.py index a00d566b..38bc99ba 100755 --- a/code/planning/src/behavior_agent/behaviours/road_features.py +++ b/code/planning/src/behavior_agent/behaviours/road_features.py @@ -268,7 +268,7 @@ def update(self): if obstacle_speed < 2 and obstacle_distance < 30: self.counter_overtake += 1 rospy.loginfo("Overtake counter: " + str(self.counter_overtake)) - if self.counter_overtake > 3: + if self.counter_overtake > 0: return py_trees.common.Status.SUCCESS return py_trees.common.Status.RUNNING else: diff --git a/code/planning/src/local_planner/ACC.py b/code/planning/src/local_planner/ACC.py index e1ac88be..00dc29d8 100755 --- a/code/planning/src/local_planner/ACC.py +++ b/code/planning/src/local_planner/ACC.py @@ -10,6 +10,7 @@ from std_msgs.msg import Float32MultiArray, Float32 from collision_check import CollisionCheck import numpy as np +from utils import interpolate_speed class ACC(CompatibleNode): @@ -173,13 +174,12 @@ def loop(timer_event=None): # https://encyclopediaofmath.org/index.php?title=Linear_interpolation safe_speed = self.obstacle_speed * \ (self.obstacle_distance / safety_distance) - lerp_factor = 0.2 - safe_speed = (1 - lerp_factor) * self.__current_velocity +\ - lerp_factor * safe_speed + + safe_speed = interpolate_speed(safe_speed, + self.__current_velocity) if safe_speed < 1.0: safe_speed = 0 - self.logerr("ACC: Safe speed: " + str(safe_speed) + - " Distance: " + str(self.obstacle_distance)) + self.logerr("ACC: Safe speed: " + str(safe_speed)) self.velocity_pub.publish(safe_speed) else: # If safety distance is reached just hold current speed @@ -197,6 +197,8 @@ def loop(timer_event=None): # If we have no obstacle, we want to drive with the current # speed limit # self.logerr("ACC: Speed limit: " + str(self.speed_limit)) + # interpolated_speed = interpolate_speed(self.speed_limit, + # self.__current_velocity) self.velocity_pub.publish(self.speed_limit) else: self.velocity_pub.publish(0) diff --git a/code/planning/src/local_planner/collision_check.py b/code/planning/src/local_planner/collision_check.py index 85540e21..6e3d6189 100755 --- a/code/planning/src/local_planner/collision_check.py +++ b/code/planning/src/local_planner/collision_check.py @@ -97,7 +97,7 @@ def __set_distance(self, data: Float32MultiArray): if nearest_object is None and \ self.__object_last_position is not None and \ rospy.get_rostime() - self.__object_last_position[0] > \ - rospy.Duration(3): + rospy.Duration(2): self.update_distance(True) return elif nearest_object is None: @@ -116,7 +116,7 @@ def __set_distance_oncoming(self, data: Float32MultiArray): if (nearest_object is None and self.__last_position_oncoming is not None and rospy.get_rostime() - self.__last_position_oncoming[0] > - rospy.Duration(3)): + rospy.Duration(1)): self.update_distance_oncoming(True) return elif nearest_object is None: @@ -151,7 +151,6 @@ def calculate_obstacle_speed(self): self.__object_last_position is None: return # If distance is np.inf no car is in front - # Calculate time since last position update rospy_time_difference = self.__object_last_position[0] - \ self.__object_first_position[0] @@ -221,11 +220,11 @@ def calculate_rule_of_thumb(emergency, speed): Returns: float: distance calculated with rule of thumb """ - reaction_distance = speed * 0.5 + reaction_distance = speed / 2 braking_distance = (speed * 0.36)**2 if emergency: # Emergency brake is really effective in Carla - return reaction_distance + braking_distance / 4 + return reaction_distance + braking_distance / 2 else: return reaction_distance + braking_distance @@ -246,8 +245,10 @@ def check_crash(self, obstacle): True, self.__current_velocity) if collision_time > 0: if distance < emergency_distance2: + # self.logerr(f"Emergency distance: {emergency_distance2}") + # self.logerr(f"Distance obstacle: {distance}") + # self.logerr(f"Obstacle speed: {obstacle_speed}") # Initiate emergency brake - self.logerr(f"Emergency Brake: {distance}") self.emergency_pub.publish(True) # When no emergency brake is needed publish collision object data = Float32MultiArray(data=[distance, obstacle_speed]) diff --git a/code/planning/src/local_planner/motion_planning.py b/code/planning/src/local_planner/motion_planning.py index 10f57135..bf29ccc3 100755 --- a/code/planning/src/local_planner/motion_planning.py +++ b/code/planning/src/local_planner/motion_planning.py @@ -17,7 +17,7 @@ import planning # noqa: F401 from behavior_agent.behaviours import behavior_speed as bs -from utils import convert_to_ms, spawn_car +from utils import convert_to_ms, spawn_car, NUM_WAYPOINTS # from scipy.spatial._kdtree import KDTree @@ -206,7 +206,7 @@ def overtake_fallback(self, distance, pose_list): # self.current_pos, # self.current_speed) selection = pose_list[int(self.current_wp):int(self.current_wp) + - int(distance) + 7] + int(distance) + NUM_WAYPOINTS] waypoints = self.convert_pose_to_array(selection) offset = np.array([2, 0, 0]) @@ -238,7 +238,7 @@ def overtake_fallback(self, distance, pose_list): path.header.stamp = rospy.Time.now() path.header.frame_id = "global" path.poses = pose_list[:int(self.current_wp)] + \ - result + pose_list[int(self.current_wp + distance + 7):] + result + pose_list[int(self.current_wp + distance + NUM_WAYPOINTS)] self.trajectory = path def __set_trajectory(self, data: Path): @@ -468,7 +468,7 @@ def __get_speed_overtake(self, behavior: str) -> float: elif behavior == bs.ot_enter_slow.name: speed = self.__calc_speed_to_stop_overtake() elif behavior == bs.ot_leave.name: - speed = convert_to_ms(10.) + speed = convert_to_ms(30.) return speed diff --git a/code/planning/src/local_planner/utils.py b/code/planning/src/local_planner/utils.py index c59675db..024f0b28 100644 --- a/code/planning/src/local_planner/utils.py +++ b/code/planning/src/local_planner/utils.py @@ -1,32 +1,21 @@ from scipy.spatial.transform import Rotation import numpy as np import math +import carla +import os # import rospy -hyperparameters = { - "max_speed": 15, - "max_accel": 4.0, - "max_curvature": 30.0, - "max_road_width_l": 4, - "max_road_width_r": 4, - "d_road_w": 0.2, - "dt": 0.2, - "maxt": 30, - "mint": 6.0, - "d_t_s": 0.5, - "n_s_sample": 2.0, - "obstacle_clearance": 1.5, - "kd": 1.0, - "kv": 0.1, - "ka": 0.1, - "kj": 0.1, - "kt": 0.1, - "ko": 0.1, - "klat": 1.0, - "klon": 1.0, - "num_threads": 1, # set 0 to avoid using threaded algorithm -} +""" +This file represents the utility functions for the local planner and other +components. +It containes parameters and utility functions to reduce code in the ros nodes. +""" + +# Number of waypoints to be used for the overtaking maneuver +NUM_WAYPOINTS = 7 +# Factor for linear interpolation of target speed values for the ACC +LERP_FACTOR = 0.5 def location_to_gps(lat_ref: float, lon_ref: float, x: float, y: float): @@ -122,8 +111,6 @@ def spawn_car(distance): Args: distance (float): distance """ - import carla - import os CARLA_HOST = os.environ.get('CARLA_HOST', 'paf23-carla-simulator-1') CARLA_PORT = int(os.environ.get('CARLA_PORT', '2000')) @@ -144,21 +131,18 @@ def spawn_car(distance): spawnPoint = carla.Transform(ego_vehicle.get_location() + carla.Location(y=distance.data), ego_vehicle.get_transform().rotation) - spawnpoint2 = carla.Transform(ego_vehicle.get_location() + - carla.Location(x=2.5, y=distance.data + 1), - ego_vehicle.get_transform().rotation) + # spawnpoint2 = carla.Transform(ego_vehicle.get_location() + + # carla.Location(x=2.5, y=distance.data + 1), + # ego_vehicle.get_transform().rotation) vehicle = world.spawn_actor(bp, spawnPoint) - vehicle2 = world.spawn_actor(bp, spawnpoint2) - vehicle2.set_autopilot(False) + # vehicle2 = world.spawn_actor(bp, spawnpoint2) + # vehicle2.set_autopilot(False) vehicle.set_autopilot(False) - # vehicle.set_location(loc) - # coords = vehicle.get_location() - # get spectator - spectator = world.get_spectator() - # set spectator to follow ego vehicle with offset - spectator.set_transform( - carla.Transform(ego_vehicle.get_location() + carla.Location(z=50), - carla.Rotation(pitch=-90))) + # vehicle.set_target_velocity(carla.Vector3D(0, 6, 0)) + + +def interpolate_speed(speed_target, speed_current): + return (1 - LERP_FACTOR) * speed_current + LERP_FACTOR * speed_target def filter_vision_objects(float_array, oncoming): From af8cd2ca00ea4b5280db061616c08a4a83eff5e2 Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Tue, 19 Mar 2024 16:47:12 +0100 Subject: [PATCH 2/2] fix: Clean up code --- build/docker-compose.yml | 2 +- .../behaviours/road_features.py | 4 +- code/planning/src/local_planner/ACC.py | 6 +- .../src/local_planner/collision_check.py | 38 +---------- .../src/local_planner/motion_planning.py | 4 +- code/planning/src/local_planner/test_traj.py | 68 ------------------- code/planning/src/local_planner/utils.py | 23 ++++++- 7 files changed, 33 insertions(+), 112 deletions(-) delete mode 100644 code/planning/src/local_planner/test_traj.py diff --git a/build/docker-compose.yml b/build/docker-compose.yml index 6c7ffe47..afc787bd 100644 --- a/build/docker-compose.yml +++ b/build/docker-compose.yml @@ -21,7 +21,7 @@ services: # based on https://github.com/ll7/paf21-1/blob/master/scenarios/docker-carla-sim-compose.yml carla-simulator: - command: /bin/bash CarlaUE4.sh -quality-level=Low -world-port=2000 -resx=800 -resy=600 -nosound -carla-settings="/home/carla/CarlaUE4/Config/CustomCarlaSettings.ini" + command: /bin/bash CarlaUE4.sh -quality-level=High -world-port=2000 -resx=800 -resy=600 -nosound -carla-settings="/home/carla/CarlaUE4/Config/CustomCarlaSettings.ini" image: ghcr.io/una-auxme/paf23:leaderboard-2.0 init: true deploy: diff --git a/code/planning/src/behavior_agent/behaviours/road_features.py b/code/planning/src/behavior_agent/behaviours/road_features.py index 38bc99ba..5ee2ac5e 100755 --- a/code/planning/src/behavior_agent/behaviours/road_features.py +++ b/code/planning/src/behavior_agent/behaviours/road_features.py @@ -5,6 +5,7 @@ import numpy as np from scipy.spatial.transform import Rotation import rospy + """ Source: https://github.com/ll7/psaf2 """ @@ -206,6 +207,7 @@ def setup(self, timeout): :return: True, as the set up is successful. """ self.blackboard = py_trees.blackboard.Blackboard() + return True def initialise(self): @@ -268,7 +270,7 @@ def update(self): if obstacle_speed < 2 and obstacle_distance < 30: self.counter_overtake += 1 rospy.loginfo("Overtake counter: " + str(self.counter_overtake)) - if self.counter_overtake > 0: + if self.counter_overtake > 3: return py_trees.common.Status.SUCCESS return py_trees.common.Status.RUNNING else: diff --git a/code/planning/src/local_planner/ACC.py b/code/planning/src/local_planner/ACC.py index 00dc29d8..e199cfd9 100755 --- a/code/planning/src/local_planner/ACC.py +++ b/code/planning/src/local_planner/ACC.py @@ -8,9 +8,8 @@ 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 numpy as np -from utils import interpolate_speed +from utils import interpolate_speed, calculate_rule_of_thumb class ACC(CompatibleNode): @@ -165,12 +164,11 @@ def loop(timer_event=None): 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( + safety_distance = 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 - # Lerp factor: # https://encyclopediaofmath.org/index.php?title=Linear_interpolation safe_speed = self.obstacle_speed * \ (self.obstacle_distance / safety_distance) diff --git a/code/planning/src/local_planner/collision_check.py b/code/planning/src/local_planner/collision_check.py index 6e3d6189..14df6e8e 100755 --- a/code/planning/src/local_planner/collision_check.py +++ b/code/planning/src/local_planner/collision_check.py @@ -11,7 +11,7 @@ # from std_msgs.msg import String from std_msgs.msg import Float32, Float32MultiArray from std_msgs.msg import Bool -from utils import filter_vision_objects +from utils import filter_vision_objects, calculate_rule_of_thumb class CollisionCheck(CompatibleNode): @@ -116,7 +116,7 @@ def __set_distance_oncoming(self, data: Float32MultiArray): if (nearest_object is None and self.__last_position_oncoming is not None and rospy.get_rostime() - self.__last_position_oncoming[0] > - rospy.Duration(1)): + rospy.Duration(2)): self.update_distance_oncoming(True) return elif nearest_object is None: @@ -208,26 +208,6 @@ def meters_to_collision(self, obstacle_speed, distance): 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 - speed (float): speed of the vehicle (km/h) - - Returns: - float: distance calculated with rule of thumb - """ - reaction_distance = speed / 2 - braking_distance = (speed * 0.36)**2 - if emergency: - # Emergency brake is really effective in Carla - 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 @@ -241,14 +221,10 @@ def check_crash(self, obstacle): collision_time = self.time_to_collision(obstacle_speed, distance) # collision_meter = self.meters_to_collision(obstacle_speed, distance) # safe_distance2 = self.calculate_rule_of_thumb(False) - emergency_distance2 = self.calculate_rule_of_thumb( + emergency_distance2 = calculate_rule_of_thumb( True, self.__current_velocity) if collision_time > 0: if distance < emergency_distance2: - # self.logerr(f"Emergency distance: {emergency_distance2}") - # self.logerr(f"Distance obstacle: {distance}") - # self.logerr(f"Obstacle speed: {obstacle_speed}") - # Initiate emergency brake self.emergency_pub.publish(True) # When no emergency brake is needed publish collision object data = Float32MultiArray(data=[distance, obstacle_speed]) @@ -263,14 +239,6 @@ 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 - # """ - # self.update_distance() - # self.calculate_obstacle_speed() - # self.new_timer(self.control_loop_rate, loop) self.spin() diff --git a/code/planning/src/local_planner/motion_planning.py b/code/planning/src/local_planner/motion_planning.py index bf29ccc3..046281e5 100755 --- a/code/planning/src/local_planner/motion_planning.py +++ b/code/planning/src/local_planner/motion_planning.py @@ -238,7 +238,8 @@ def overtake_fallback(self, distance, pose_list): path.header.stamp = rospy.Time.now() path.header.frame_id = "global" path.poses = pose_list[:int(self.current_wp)] + \ - result + pose_list[int(self.current_wp + distance + NUM_WAYPOINTS)] + result + pose_list[int(self.current_wp + distance + + NUM_WAYPOINTS):] self.trajectory = path def __set_trajectory(self, data: Path): @@ -469,7 +470,6 @@ def __get_speed_overtake(self, behavior: str) -> float: speed = self.__calc_speed_to_stop_overtake() elif behavior == bs.ot_leave.name: speed = convert_to_ms(30.) - return speed def __get_speed_cruise(self) -> float: diff --git a/code/planning/src/local_planner/test_traj.py b/code/planning/src/local_planner/test_traj.py deleted file mode 100644 index 05a00c73..00000000 --- a/code/planning/src/local_planner/test_traj.py +++ /dev/null @@ -1,68 +0,0 @@ -import numpy as np - -from frenet_optimal_trajectory_planner.FrenetOptimalTrajectory.fot_wrapper \ - import run_fot - -import matplotlib.pyplot as plt - - -wp = np.r_[[np.full((50), 983.5889666959667)], - [np.linspace(5370.016106881272, 5399.016106881272, 50)]].T -obs = np.array([[983.568124548765, 5384.0219828457075, - 983.628124548765, 5386.0219828457075]]) -initial_conditions = { - 'ps': 0, - 'target_speed': 6, - 'pos': np.array([983.5807552562393, 5370.014637890163]), - 'vel': np.array([5, 1]), - 'wp': wp, - 'obs': obs -} - -hyperparameters = { - "max_speed": 25.0, - "max_accel": 15.0, - "max_curvature": 20.0, - "max_road_width_l": 3.0, - "max_road_width_r": 0, - "d_road_w": 0.5, - "dt": 0.2, - "maxt": 20.0, - "mint": 6.0, - "d_t_s": 0.5, - "n_s_sample": 2.0, - "obstacle_clearance": 2, - "kd": 1.0, - "kv": 0.1, - "ka": 0.1, - "kj": 0.1, - "kt": 0.1, - "ko": 0.1, - "klat": 1.0, - "klon": 1.0, - "num_threads": 0, # set 0 to avoid using threaded algorithm -} - -result_x, result_y, speeds, ix, iy, iyaw, d, s, speeds_x, \ - speeds_y, misc, costs, success = run_fot(initial_conditions, - hyperparameters) - -if success: - print("Success!") - print("result_x: ", result_x) - print("result_y: ", result_y) - print("yaw!", iyaw) - fig, ax = plt.subplots(1, 2) - - ax[0].scatter(wp[:, 0], wp[:, 1], label="original") - ax[0].scatter([obs[0, 0], obs[0, 2]], - [obs[0, 1], obs[0, 3]], label="object") - ax[0].set_xticks([obs[0, 0], obs[0, 2]]) - ax[1].scatter(result_x, result_y, label="frenet") - ax[1].scatter([obs[0, 0], obs[0, 2]], - [obs[0, 1], obs[0, 3]], label="object") - ax[1].set_xticks([obs[0, 0], obs[0, 2]]) - plt.legend() - plt.show() -else: - print("Failure!") diff --git a/code/planning/src/local_planner/utils.py b/code/planning/src/local_planner/utils.py index 024f0b28..ecf88b2f 100644 --- a/code/planning/src/local_planner/utils.py +++ b/code/planning/src/local_planner/utils.py @@ -16,6 +16,8 @@ NUM_WAYPOINTS = 7 # Factor for linear interpolation of target speed values for the ACC LERP_FACTOR = 0.5 +# Earth radius in meters for location_to_GPS +EARTH_RADIUS_EQUA = 6378137.0 def location_to_gps(lat_ref: float, lon_ref: float, x: float, y: float): @@ -33,7 +35,6 @@ def location_to_gps(lat_ref: float, lon_ref: float, x: float, y: float): dict: Dictionary with (lat,lon,z) coordinates """ - EARTH_RADIUS_EQUA = 6378137.0 # pylint: disable=invalid-name scale = math.cos(lat_ref * math.pi / 180.0) mx = scale * lon_ref * math.pi * EARTH_RADIUS_EQUA / 180.0 my = scale * EARTH_RADIUS_EQUA * math.log(math.tan((90.0 + lat_ref) * @@ -49,6 +50,26 @@ def location_to_gps(lat_ref: float, lon_ref: float, x: float, y: float): return {'lat': lat, 'lon': lon, 'z': z} +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 + speed (float): speed of the vehicle (km/h) + + Returns: + float: distance calculated with rule of thumb + """ + reaction_distance = speed / 2 + braking_distance = (speed * 0.36)**2 + if emergency: + # Emergency brake is really effective in Carla + return reaction_distance + braking_distance + else: + return reaction_distance + braking_distance + + def approx_obstacle_pos(distance: float, heading: float, ego_pos: np.array, speed: float): """calculate the position of the obstacle in the global coordinate system