From f93dadfdea042e91056123e1b5604074cb74c7d9 Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Sun, 17 Dec 2023 13:49:22 +0100 Subject: [PATCH] fix: removed test code --- .../local_planner/launch/local_planner.launch | 4 +-- code/planning/local_planner/src/ACC.py | 13 +++------ .../local_planner/src/collision_check.py | 29 +++++-------------- .../src/dev_collision_publisher.py | 3 +- 4 files changed, 14 insertions(+), 35 deletions(-) diff --git a/code/planning/local_planner/launch/local_planner.launch b/code/planning/local_planner/launch/local_planner.launch index 955f3d44..a27956cd 100644 --- a/code/planning/local_planner/launch/local_planner.launch +++ b/code/planning/local_planner/launch/local_planner.launch @@ -7,8 +7,8 @@ - + diff --git a/code/planning/local_planner/src/ACC.py b/code/planning/local_planner/src/ACC.py index befb6ca8..cde7195a 100755 --- a/code/planning/local_planner/src/ACC.py +++ b/code/planning/local_planner/src/ACC.py @@ -23,7 +23,7 @@ def __init__(self): self.control_loop_rate = self.get_param("control_loop_rate", 1) self.current_speed = 50 / 3.6 # m/ss - self.logerr("ACC started") + self.loginfo("ACC started") # TODO: Add Subscriber for Obsdacle from Collision Check self.collision_sub = self.new_subscription( Float32MultiArray, @@ -33,8 +33,8 @@ def __init__(self): # Get current speed self.velocity_sub: Subscriber = self.new_subscription( - Float32, - f"/paf/{self.role_name}/test_speed", + CarlaSpeedometer, + f"/carla/{self.role_name}/Speed", self.__get_current_velocity, qos_profile=1) @@ -90,7 +90,6 @@ def __get_collision(self, data: Float32MultiArray): if np.isinf(data.data[0]): # No collision ahead self.collision_ahead = False - self.logerr("No Collision ahead -> ACC") else: # Collision ahead self.collision_ahead = True @@ -113,7 +112,6 @@ def calculate_safe_speed(self): # Calculate safety distance safety_distance = CollisionCheck.calculate_rule_of_thumb( False, self.__current_velocity) - self.logerr("Safety Distance: " + str(safety_distance)) if self.obstacle[0] < safety_distance: # If safety distance is reached, we want to reduce the speed to # meet the desired distance @@ -124,7 +122,6 @@ def calculate_safe_speed(self): safe_speed = self.obstacle[1] * (self.obstacle[0] / safety_distance) - self.logerr("Safe Speed: " + str(safe_speed)) return safe_speed else: # If safety distance is reached, drive with same speed as @@ -132,8 +129,6 @@ def calculate_safe_speed(self): # TODO: # Incooperate overtaking -> # Communicate with decision tree about overtaking - self.logerr("saftey distance good; Speed from obstacle: " + - str(self.obstacle[1])) return self.obstacle[1] def __get_current_velocity(self, data: CarlaSpeedometer): @@ -142,7 +137,7 @@ def __get_current_velocity(self, data: CarlaSpeedometer): Args: data (CarlaSpeedometer): _description_ """ - self.__current_velocity = float(data.data) + self.__current_velocity = float(data.speed) def __set_trajectory(self, data: Path): """Recieve trajectory from global planner diff --git a/code/planning/local_planner/src/collision_check.py b/code/planning/local_planner/src/collision_check.py index 0aa3a9f1..db4e6add 100755 --- a/code/planning/local_planner/src/collision_check.py +++ b/code/planning/local_planner/src/collision_check.py @@ -6,7 +6,7 @@ from ros_compatibility.node import CompatibleNode from rospy import Subscriber from geometry_msgs.msg import PoseStamped -# from carla_msgs.msg import CarlaSpeedometer # , CarlaWorldInfo +from carla_msgs.msg import CarlaSpeedometer # , CarlaWorldInfo # from std_msgs.msg import String from std_msgs.msg import Float32, Float32MultiArray from std_msgs.msg import Bool @@ -25,14 +25,14 @@ def __init__(self): self.control_loop_rate = self.get_param("control_loop_rate", 1) # self.current_speed = 50 / 3.6 # m/ss # TODO: Add Subscriber for Speed and Obstacles - self.logerr("CollisionCheck started") + self.loginfo("CollisionCheck started") # self.obstacle_sub: Subscriber = self.new_subscription( # ) # Subscriber for current speed self.velocity_sub: Subscriber = self.new_subscription( - Float32, # CarlaSpeedometer # f"/carla/{self.role_name}/Speed" - f"/paf/{self.role_name}/test_speed", + CarlaSpeedometer, + f"/carla/{self.role_name}/Speed", self.__get_current_velocity, qos_profile=1) # Subscriber for current position @@ -44,7 +44,7 @@ def __init__(self): # Subscriber for lidar distance self.lidar_dist = self.new_subscription( Float32, - f"/carla/{self.role_name}/lidar_dist_dev", + f"/carla/{self.role_name}/lidar_dist_dev", # TODO: Change to lidar topic self.calculate_obstacle_speed, qos_profile=1) # Publisher for emergency stop @@ -71,15 +71,12 @@ def calculate_obstacle_speed(self, new_dist: Float32): """ # Check if current speed from vehicle is not None if self.__current_velocity is None: - self.logerr("Current Speed is None") return # Check if this is the first time the callback is called - self.logerr("distance recieved: " + str(new_dist.data)) if self.__object_last_position is None and \ np.isinf(new_dist.data) is not True: self.__object_last_position = (time.time(), new_dist.data) - self.logerr("First Position") return # If distance is np.inf no car is in front @@ -87,14 +84,12 @@ def calculate_obstacle_speed(self, new_dist: Float32): self.__object_last_position = None data = Float32MultiArray(data=[np.Inf, -1]) self.collision_pub.publish(data) - self.logerr("No car in front: dist recieved is inf") 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.data) - self.logerr("Time difference too big") return # Calculate time since last position update current_time = time.time() @@ -104,25 +99,20 @@ def calculate_obstacle_speed(self, new_dist: Float32): distance = new_dist.data - self.__object_last_position[1] # Speed is distance/time (m/s) - self.logerr("Time Difference: " + str(time_difference)) - self.logerr("Distance difference: " + str(distance)) relative_speed = distance/time_difference - self.logerr("Relative Speed: " + str(relative_speed)) - self.logerr("Current Speed: " + str(self.__current_velocity)) speed = self.__current_velocity + relative_speed - self.logerr("Speed: " + str(speed)) # Check for crash self.check_crash((new_dist.data, speed)) self.__object_last_position = (current_time, new_dist.data) - def __get_current_velocity(self, data: Float32): + 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.data) + self.__current_velocity = float(data.speed) def __current_position_callback(self, data: PoseStamped): """Saves current position of the ego vehicle @@ -189,28 +179,23 @@ def check_crash(self, obstacle): collision_time = self.time_to_collision(obstacle_speed, distance) # collision_meter = self.meters_to_collision(obstacle_speed, distance) - self.logerr("Collision Time: " + str(collision_time)) # safe_distance2 = self.calculate_rule_of_thumb(False) emergency_distance2 = self.calculate_rule_of_thumb( True, self.__current_velocity) - self.logerr("Emergency Distance: " + str(emergency_distance2)) if collision_time > 0: if distance < emergency_distance2: # Initiate emergency brake self.emergency_pub.publish(True) - self.logerr("Emergency Brake") 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) - self.logerr("Collision published") # print(f"Safe Distance Thumb: {safe_distance2:.2f}") else: # If no collision is ahead publish np.Inf data = Float32MultiArray(data=[np.Inf, obstacle_speed]) self.collision_pub.publish(data) - self.logerr("No Collision ahead") def run(self): """ diff --git a/code/planning/local_planner/src/dev_collision_publisher.py b/code/planning/local_planner/src/dev_collision_publisher.py index fe8c3368..65fb1c2e 100755 --- a/code/planning/local_planner/src/dev_collision_publisher.py +++ b/code/planning/local_planner/src/dev_collision_publisher.py @@ -44,7 +44,7 @@ def __init__(self): topic='/paf/' + self.role_name + '/manual', callback=self.callback_manual, qos_profile=1) - self.logerr("DevCollisionCheck started") + self.loginfo("DevCollisionCheck started") self.last_position_update = None self.simulated_speed = 12 # m/s self.distance_to_collision = 0 @@ -54,7 +54,6 @@ def __init__(self): def callback_manual(self, msg: Float32): if self.manual_start: - self.logerr("Manual start") self.manual_start = False self.pub_lidar.publish(Float32(data=25)) time.sleep(0.2)