From d7a5daa6fdcf0018df146874666b7c5ad14e54b7 Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Thu, 7 Dec 2023 17:02:31 +0100 Subject: [PATCH 01/18] feat: added speed calculation for Object --- .../local_planner/src/collision_check.py | 121 ++++++++++++++---- 1 file changed, 98 insertions(+), 23 deletions(-) diff --git a/code/planning/local_planner/src/collision_check.py b/code/planning/local_planner/src/collision_check.py index 34bcdbb4..72173567 100755 --- a/code/planning/local_planner/src/collision_check.py +++ b/code/planning/local_planner/src/collision_check.py @@ -1,14 +1,15 @@ #!/usr/bin/env python -# import rospy +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 Publisher, Subscriber, Duration +from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion +from carla_msgs.msg import CarlaRoute, CarlaSpeedometer # , CarlaWorldInfo +from nav_msgs.msg import Path # from std_msgs.msg import String -# from std_msgs.msg import Float32MultiArray +from std_msgs.msg import Float32MultiArray class CollisionCheck(CompatibleNode): @@ -25,6 +26,81 @@ def __init__(self): # TODO: Add Subscriber for Speed and Obstacles self.logdebug("CollisionCheck started") + # self.obstacle_sub: Subscriber = self.new_subscription( + # ) + self.velocity_sub: Subscriber = self.new_subscription( + CarlaSpeedometer, + f"/carla/{self.role_name}/Speed", + self.__get_current_velocity, + qos_profile=1) + + 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) + + # needed to prevent the car from driving before a path to follow is + # available. Might be needed later to slow down in curves + self.trajectory_sub: Subscriber = self.new_subscription( + Path, + f"/paf/{self.role_name}/trajectory", + self.__set_trajectory, + qos_profile=1) + + self.current_pos_sub: Subscriber = self.new_subscription( + msg_type=PoseStamped, + topic="/paf/" + self.role_name + "/current_pos", + callback=self.__current_position_callback, + qos_profile=1) + + self.__speed_limits_OD: [float] = [] + self.__trajectory: Path = None + self.__current_wp_index: int = 0 + self.__current_velocity: float = None + self.__object_last_position: tuple = None + + def calculate_obstacle_speed(self, new_position): + # Calculate time since last position update + current_time = rospy.get_rostime() + time_difference = current_time-self.__object_last_position[0] + + # Calculate distance (in m) - Euclidian distance is used as approx + distance = np.linalg.norm( + new_position - self.__object_last_position[1]) + + # Speed is distance/time (m/s) + return distance/time_difference + + def __get_current_velocity(self, data: CarlaSpeedometer): + self.__current_velocity = float(data.speed) + self.velocity_pub.publish(self.__current_velocity) + + def __set_trajectory(self, data: Path): + self.__trajectory = data + + def __set_speed_limits_opendrive(self, data: Float32MultiArray): + self.__speed_limits_OD = data.data + + def __current_position_callback(self, data: PoseStamped): + 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 update(self, speed): self.current_speed = speed @@ -35,20 +111,6 @@ def meters_to_collision(self, obstacle_speed, distance): return self.time_to_collision(obstacle_speed, distance) * \ self.current_speed - # PAF 22 - def calculate_safe_dist(self) -> float: - """ - 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 - """ - 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 @@ -63,7 +125,6 @@ def check_crash(self, 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) @@ -73,7 +134,6 @@ def check_crash(self, obstacle): 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}") else: print("Ego slower then car in front") @@ -85,7 +145,22 @@ def run(self): """ def loop(timer_event=None): - pass + if self.__velocity is None: + self.logdebug("ACC hasn't received the velocity of the ego " + "vehicle yet and can therefore not publish a " + "velocity") + return + + if self.__dist < 0.5: + self.velocity_pub.publish(0) + self.logwarn("ACC off") + self.__on = False + self.__dist = None # to check if new dist was published + return + + # Use for testing + # self.d_dist_pub.publish(self.calculate_optimal_dist()-self.__dist) + # self.velocity_pub.publish(v) self.new_timer(self.control_loop_rate, loop) self.spin() From cda33e0c028d23f347e05c4ccd16b41b42eb8142 Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Thu, 7 Dec 2023 17:03:03 +0100 Subject: [PATCH 02/18] fix: unused imports --- code/planning/local_planner/src/collision_check.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/code/planning/local_planner/src/collision_check.py b/code/planning/local_planner/src/collision_check.py index 72173567..7368ec23 100755 --- a/code/planning/local_planner/src/collision_check.py +++ b/code/planning/local_planner/src/collision_check.py @@ -4,9 +4,9 @@ # import tf.transformations import ros_compatibility as roscomp from ros_compatibility.node import CompatibleNode -from rospy import Publisher, Subscriber, Duration -from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion -from carla_msgs.msg import CarlaRoute, CarlaSpeedometer # , CarlaWorldInfo +from rospy import Subscriber +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 From 35bc1048a0feea9730d6d17d229b7676808574e2 Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Fri, 8 Dec 2023 17:42:13 +0100 Subject: [PATCH 03/18] feat: ACC Node added, Collision check cleared --- .../local_planner/launch/local_planner.launch | 4 + code/planning/local_planner/src/ACC.py | 186 ++++++++++++++++++ .../local_planner/src/collision_check.py | 62 +----- 3 files changed, 193 insertions(+), 59 deletions(-) create mode 100644 code/planning/local_planner/src/ACC.py diff --git a/code/planning/local_planner/launch/local_planner.launch b/code/planning/local_planner/launch/local_planner.launch index c469c05e..8cf61195 100644 --- a/code/planning/local_planner/launch/local_planner.launch +++ b/code/planning/local_planner/launch/local_planner.launch @@ -3,4 +3,8 @@ + + + + diff --git a/code/planning/local_planner/src/ACC.py b/code/planning/local_planner/src/ACC.py new file mode 100644 index 00000000..46b3167a --- /dev/null +++ b/code/planning/local_planner/src/ACC.py @@ -0,0 +1,186 @@ +#!/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 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 + + +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 + + self.logdebug("ACC started") + # TODO: Add Subscriber for Obsdacle from Collision Check + # self.obstacle_sub: Subscriber = self.new_subscription( + # ) + + # Get current speed + self.velocity_sub: Subscriber = self.new_subscription( + CarlaSpeedometer, + f"/carla/{self.role_name}/Speed", + self.__get_current_velocity, + 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) + + # Get current position to determine current speed limit + self.current_pos_sub: Subscriber = self.new_subscription( + msg_type=PoseStamped, + topic="/paf/" + self.role_name + "/current_pos", + callback=self.__current_position_callback, + qos_profile=1) + + # Publish desiored 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 + # Is an obstacle ahead where we would collide with? + self.collision_ahead: bool = False + # Distnace and speed from possible collsion object + self.__obstacle: tuple = None + # Current speed limit + self.speed_limit: float = np.Inf + + + def calculate_safe_speed(self, distance, object_speed, own_speed): + """calculates the speed to meet the desired distance to the object + + Args: + distance (float): Distance to the object in front + object_speed (float): Speed from object in front + own_speed (float): Current speed of the ego vehicle + + Returns: + float: safe speed tp meet the desired distance + """ + safety_distance = own_speed/2 + if distance < safety_distance: + # If safety distance is reached, we want to reduce the speed to meet the desired distance + # The speed is reduced by the factor of the distance to the safety distance + # Another solution could be object_speed - (safety_distance-distance) + + safe_speed = object_speed * (distance / safety_distance) + return safe_speed + else: + # If safety distance is reached, drive with same speed as Object in front + # TODO: Incooperate overtaking -> Communicate with decision tree about overtaking + return object_speed + + def __get_current_velocity(self, data: CarlaSpeedometer): + """_summary_ + + Args: + data (CarlaSpeedometer): _description_ + """ + self.__current_velocity = float(data.speed) + self.velocity_pub.publish(self.__current_velocity) + + def __set_trajectory(self, data: Path): + self.__trajectory = data + + def __set_speed_limits_opendrive(self, data: Float32MultiArray): + self.__speed_limits_OD = data.data + + def __current_position_callback(self, data: PoseStamped): + 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): + if self.__velocity is None: + self.logdebug("ACC hasn't received the velocity of the ego " + "vehicle yet and can therefore not publish a " + "velocity") + return + + # check if collision is ahead + if self.collision_ahead: + # collision is ahead + # check if object moves + if self.obstacle_speed > 0: + # Object is moving + # caluculate safe speed + speed = self.calculate_safe_speed() + self.velocity_pub.publish(speed) + else: + # If object doesnt move, behaviour tree will handle overtaking or emergency stop was done by collision check + pass + else: + # no collisoion ahead -> publish 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() + + print("ACC") \ No newline at end of file diff --git a/code/planning/local_planner/src/collision_check.py b/code/planning/local_planner/src/collision_check.py index 7368ec23..e0f0c25e 100755 --- a/code/planning/local_planner/src/collision_check.py +++ b/code/planning/local_planner/src/collision_check.py @@ -34,31 +34,15 @@ def __init__(self): self.__get_current_velocity, qos_profile=1) - 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) - - # needed to prevent the car from driving before a path to follow is - # available. Might be needed later to slow down in curves - self.trajectory_sub: Subscriber = self.new_subscription( - Path, - f"/paf/{self.role_name}/trajectory", - self.__set_trajectory, - qos_profile=1) - self.current_pos_sub: Subscriber = self.new_subscription( msg_type=PoseStamped, topic="/paf/" + self.role_name + "/current_pos", callback=self.__current_position_callback, qos_profile=1) - self.__speed_limits_OD: [float] = [] - self.__trajectory: Path = None - self.__current_wp_index: int = 0 self.__current_velocity: float = None self.__object_last_position: tuple = None + self._current_position: tuple= None def calculate_obstacle_speed(self, new_position): # Calculate time since last position update @@ -76,33 +60,8 @@ def __get_current_velocity(self, data: CarlaSpeedometer): self.__current_velocity = float(data.speed) self.velocity_pub.publish(self.__current_velocity) - def __set_trajectory(self, data: Path): - self.__trajectory = data - - def __set_speed_limits_opendrive(self, data: Float32MultiArray): - self.__speed_limits_OD = data.data - def __current_position_callback(self, data: PoseStamped): - 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 update(self, speed): - self.current_speed = speed + self._current_position = (data.pose.position.x, data.pose.position.y) def time_to_collision(self, obstacle_speed, distance): return distance / (self.current_speed - obstacle_speed) @@ -145,22 +104,7 @@ def run(self): """ def loop(timer_event=None): - if self.__velocity is None: - self.logdebug("ACC hasn't received the velocity of the ego " - "vehicle yet and can therefore not publish a " - "velocity") - return - - if self.__dist < 0.5: - self.velocity_pub.publish(0) - self.logwarn("ACC off") - self.__on = False - self.__dist = None # to check if new dist was published - return - - # Use for testing - # self.d_dist_pub.publish(self.calculate_optimal_dist()-self.__dist) - # self.velocity_pub.publish(v) + pass self.new_timer(self.control_loop_rate, loop) self.spin() From 91e519e91a0be68e73150717bddce059d377e62c Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Mon, 11 Dec 2023 17:06:46 +0100 Subject: [PATCH 04/18] feat: Collision Check node finished and ACC publishers and subscribers created --- code/acting/launch/acting.launch | 4 +- code/planning/local_planner/src/ACC.py | 97 ++++++++------ .../local_planner/src/collision_check.py | 123 +++++++++++++++--- 3 files changed, 166 insertions(+), 58 deletions(-) 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/local_planner/src/ACC.py b/code/planning/local_planner/src/ACC.py index 46b3167a..08f74cf3 100644 --- a/code/planning/local_planner/src/ACC.py +++ b/code/planning/local_planner/src/ACC.py @@ -1,8 +1,7 @@ #!/usr/bin/env python -import rospy import numpy as np +import roscomp # import tf.transformations -import ros_compatibility as roscomp from ros_compatibility.node import CompatibleNode from rospy import Subscriber, Publisher from geometry_msgs.msg import PoseStamped @@ -14,7 +13,7 @@ class ACC(CompatibleNode): """ - This node recieves a possible collision and + This node recieves a possible collision and """ def __init__(self): @@ -22,11 +21,14 @@ def __init__(self): 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.logdebug("ACC started") # TODO: Add Subscriber for Obsdacle from Collision Check - # self.obstacle_sub: Subscriber = self.new_subscription( - # ) + self.collision_sub = self.new_subscription( + Float32MultiArray, + f"/paf/{self.role_name}/collision", + self.__get_collision, + qos_profile=1) # Get current speed self.velocity_sub: Subscriber = self.new_subscription( @@ -72,35 +74,55 @@ def __init__(self): self.__current_velocity: float = None # Is an obstacle ahead where we would collide with? self.collision_ahead: bool = False - # Distnace and speed from possible collsion object - self.__obstacle: tuple = None + # Distance and speed from possible collsion object + self.obstacle: tuple = None # Current speed limit self.speed_limit: float = np.Inf - - def calculate_safe_speed(self, distance, object_speed, own_speed): - """calculates the speed to meet the desired distance to the object + def __get_collision(self, data: Float32MultiArray): + """Check if collision is ahead Args: - distance (float): Distance to the object in front - object_speed (float): Speed from object in front - own_speed (float): Current speed of the ego vehicle + data (Float32MultiArray): Distance and speed from possible + collsion object + """ + if data.data[1] == np.Inf: + # No collision ahead + self.collision_ahead = False + else: + # Collision ahead + self.collision_ahead = True + self.obstacle = (data.data[0], data.data[1]) + self.calculate_safe_speed() + + def calculate_safe_speed(self): + """calculates the speed to meet the desired distance to the object Returns: float: safe speed tp meet the desired distance """ - safety_distance = own_speed/2 - if distance < safety_distance: - # If safety distance is reached, we want to reduce the speed to meet the desired distance - # The speed is reduced by the factor of the distance to the safety distance - # Another solution could be object_speed - (safety_distance-distance) - - safe_speed = object_speed * (distance / safety_distance) + # 1s * m/s = reaction distance + reaction_distance = self.__current_velocity + safety_distance = reaction_distance + \ + (self.__current_velocity * 0.36)**2 + if self.obstacle[0] < safety_distance: + # If safety distance is reached, we want to reduce the speed to + # meet the desired distance + # Speed is reduced by the factor of the distance to the safety + # distance + # Another solution could be + # object_speed - (safety_distance-distance) + + safe_speed = self.obstacle[1] * (self.obstacle[0] / + safety_distance) return safe_speed else: - # If safety distance is reached, drive with same speed as Object in front - # TODO: Incooperate overtaking -> Communicate with decision tree about overtaking - return object_speed + # If safety distance is reached, drive with same speed as + # Object in front + # TODO: + # Incooperate overtaking -> + # Communicate with decision tree about overtaking + return self.obstacle[1] def __get_current_velocity(self, data: CarlaSpeedometer): """_summary_ @@ -148,8 +170,8 @@ def loop(timer_event=None): "vehicle yet and can therefore not publish a " "velocity") return - - # check if collision is ahead + + # check if collision is ahead if self.collision_ahead: # collision is ahead # check if object moves @@ -159,7 +181,8 @@ def loop(timer_event=None): speed = self.calculate_safe_speed() self.velocity_pub.publish(speed) else: - # If object doesnt move, behaviour tree will handle overtaking or emergency stop was done by collision check + # If object doesnt move, behaviour tree will handle + # overtaking or emergency stop was done by collision check pass else: # no collisoion ahead -> publish speed limit @@ -173,14 +196,12 @@ def loop(timer_event=None): main function starts the ACC node :param args: """ - # roscomp.init('ACC') - - # try: - # node = ACC() - # node.run() - # except KeyboardInterrupt: - # pass - # finally: - # roscomp.shutdown() - - print("ACC") \ No newline at end of file + 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 e0f0c25e..9da23271 100755 --- a/code/planning/local_planner/src/collision_check.py +++ b/code/planning/local_planner/src/collision_check.py @@ -7,9 +7,9 @@ from rospy import Subscriber 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 +from std_msgs.msg import Float32, Float32MultiArray +from std_msgs.msg import Bool class CollisionCheck(CompatibleNode): @@ -28,49 +28,124 @@ def __init__(self): # 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 current position self.current_pos_sub: Subscriber = self.new_subscription( msg_type=PoseStamped, topic="/paf/" + self.role_name + "/current_pos", callback=self.__current_position_callback, qos_profile=1) - + # Subscriber for lidar distance + self.lidar_dist = self.new_subscription( + Float32, + f"/carla/{self.role_name}/lidar_dist", + 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) + # Variables to save vehicle data self.__current_velocity: float = None self.__object_last_position: tuple = None - self._current_position: tuple= None + self._current_position: tuple = None + + def calculate_obstacle_speed(self, new_position: Float32): + """Caluclate the speed of the obstacle in front of the ego vehicle + based on the distance between to timestamps - def calculate_obstacle_speed(self, new_position): + Args: + new_position (Float32): new position received from the lidar + """ + # Check if this is the first time the callback is called + if self.__object_last_position is None: + self.__object_last_position = (rospy.get_rostime(), + new_position.data) + return + + # If speed is np.inf no car is in front + if new_position.data == np.inf: + self.__object_last_position = None + return + # Check if too much time has passed since last position update + if self.__object_last_position[0] + \ + rospy.Duration(0.5) < rospy.get_rostime(): + self.__object_last_position = None + return # Calculate time since last position update current_time = rospy.get_rostime() time_difference = current_time-self.__object_last_position[0] - # Calculate distance (in m) - Euclidian distance is used as approx - distance = np.linalg.norm( - new_position - self.__object_last_position[1]) + # Calculate distance (in m) + distance = new_position.data - self.__object_last_position[1] # Speed is distance/time (m/s) - return distance/time_difference + speed = distance/time_difference + self.check_crash((distance, speed)) 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) self.velocity_pub.publish(self.__current_velocity) def __current_position_callback(self, data: PoseStamped): + """Saves current position of the ego vehicle + + Args: + data (PoseStamped): Message from Perception with current position + """ self._current_position = (data.pose.position.x, data.pose.position.y) def time_to_collision(self, obstacle_speed, distance): + """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 + """ return distance / (self.current_speed - obstacle_speed) def meters_to_collision(self, obstacle_speed, distance): + """Calculates the meters until collision with the obstacle in front + + 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 + """ return self.time_to_collision(obstacle_speed, distance) * \ - self.current_speed + self.self.__current_velocity def calculate_rule_of_thumb(self, emergency): + """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 + """ reaction_distance = self.current_speed braking_distance = (self.current_speed * 0.36)**2 if emergency: @@ -79,23 +154,35 @@ def calculate_rule_of_thumb(self, emergency): 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_distance2 = self.calculate_rule_of_thumb(False) + # safe_distance2 = self.calculate_rule_of_thumb(False) emergency_distance2 = self.calculate_rule_of_thumb(True) - # TODO: Convert to Publishers 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 Thumb: {safe_distance2:.2f}") + # 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=[collision_meter, obstacle_speed]) + self.collision_pub.publish(data) + # print(f"Safe Distance Thumb: {safe_distance2:.2f}") else: - print("Ego slower then car in front") + # If no collision is ahead publish np.Inf + data = Float32MultiArray(data=[np.Inf, -1]) + self.collision_pub(data) def run(self): """ From aeb38352e5322e5622923c28385670f27d8e84b4 Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Mon, 11 Dec 2023 22:41:44 +0100 Subject: [PATCH 05/18] fix: Comments and run function --- code/planning/local_planner/src/ACC.py | 25 ++++++++++++++++++- .../local_planner/src/collision_check.py | 5 ---- 2 files changed, 24 insertions(+), 6 deletions(-) diff --git a/code/planning/local_planner/src/ACC.py b/code/planning/local_planner/src/ACC.py index 08f74cf3..c8202ff8 100644 --- a/code/planning/local_planner/src/ACC.py +++ b/code/planning/local_planner/src/ACC.py @@ -93,7 +93,9 @@ def __get_collision(self, data: Float32MultiArray): # Collision ahead self.collision_ahead = True self.obstacle = (data.data[0], data.data[1]) - self.calculate_safe_speed() + target_speed = self.calculate_safe_speed() + if target_speed is not None: + self.velocity_pub.publish(target_speed) def calculate_safe_speed(self): """calculates the speed to meet the desired distance to the object @@ -101,6 +103,11 @@ def calculate_safe_speed(self): Returns: float: safe speed tp meet the desired distance """ + # No speed or obstacle recieved yet + if self.__current_velocity is None: + return None + if self.obstacle is None: + return None # 1s * m/s = reaction distance reaction_distance = self.__current_velocity safety_distance = reaction_distance + \ @@ -134,12 +141,28 @@ def __get_current_velocity(self, data: CarlaSpeedometer): self.velocity_pub.publish(self.__current_velocity) 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 diff --git a/code/planning/local_planner/src/collision_check.py b/code/planning/local_planner/src/collision_check.py index 9da23271..f52b6b34 100755 --- a/code/planning/local_planner/src/collision_check.py +++ b/code/planning/local_planner/src/collision_check.py @@ -189,11 +189,6 @@ def run(self): Control loop :return: """ - - def loop(timer_event=None): - pass - - self.new_timer(self.control_loop_rate, loop) self.spin() From 273f7466374f3cc75a28b55c5fd9c15c624112d0 Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Tue, 12 Dec 2023 16:44:38 +0100 Subject: [PATCH 06/18] feat: dev env created --- build/docker-compose.yml | 4 +- .../launch/global_planner.launch | 6 +- .../local_planner/launch/local_planner.launch | 6 +- code/planning/local_planner/src/ACC.py | 29 +---- .../local_planner/src/collision_check.py | 18 ++- .../src/dev_collision_publisher.py | 103 ++++++++++++++++++ 6 files changed, 132 insertions(+), 34 deletions(-) mode change 100644 => 100755 code/planning/local_planner/src/ACC.py create mode 100755 code/planning/local_planner/src/dev_collision_publisher.py diff --git a/build/docker-compose.yml b/build/docker-compose.yml index a68e97c4..47e13068 100644 --- a/build/docker-compose.yml +++ b/build/docker-compose.yml @@ -57,8 +57,8 @@ 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/planning/global_planner/launch/global_planner.launch b/code/planning/global_planner/launch/global_planner.launch index d26b4873..8d65c20c 100644 --- a/code/planning/global_planner/launch/global_planner.launch +++ b/code/planning/global_planner/launch/global_planner.launch @@ -1,12 +1,12 @@ - + diff --git a/code/planning/local_planner/launch/local_planner.launch b/code/planning/local_planner/launch/local_planner.launch index 8cf61195..9aa9f4de 100644 --- a/code/planning/local_planner/launch/local_planner.launch +++ b/code/planning/local_planner/launch/local_planner.launch @@ -6,5 +6,9 @@ - + + + + + diff --git a/code/planning/local_planner/src/ACC.py b/code/planning/local_planner/src/ACC.py old mode 100644 new mode 100755 index c8202ff8..679b5566 --- a/code/planning/local_planner/src/ACC.py +++ b/code/planning/local_planner/src/ACC.py @@ -1,6 +1,6 @@ #!/usr/bin/env python import numpy as np -import roscomp +import ros_compatibility as roscomp # import tf.transformations from ros_compatibility.node import CompatibleNode from rospy import Subscriber, Publisher @@ -122,6 +122,7 @@ def calculate_safe_speed(self): safe_speed = self.obstacle[1] * (self.obstacle[0] / safety_distance) + self.logdebug("Safe Speed: " + str(safe_speed)) return safe_speed else: # If safety distance is reached, drive with same speed as @@ -129,6 +130,8 @@ def calculate_safe_speed(self): # TODO: # Incooperate overtaking -> # Communicate with decision tree about overtaking + self.logdebug("saftey distance gooood; Speed from obstacle: " + + str(self.obstacle[1])) return self.obstacle[1] def __get_current_velocity(self, data: CarlaSpeedometer): @@ -138,7 +141,6 @@ def __get_current_velocity(self, data: CarlaSpeedometer): data (CarlaSpeedometer): _description_ """ self.__current_velocity = float(data.speed) - self.velocity_pub.publish(self.__current_velocity) def __set_trajectory(self, data: Path): """Recieve trajectory from global planner @@ -186,29 +188,8 @@ def run(self): Control loop :return: """ - def loop(timer_event=None): - if self.__velocity is None: - self.logdebug("ACC hasn't received the velocity of the ego " - "vehicle yet and can therefore not publish a " - "velocity") - return - - # check if collision is ahead - if self.collision_ahead: - # collision is ahead - # check if object moves - if self.obstacle_speed > 0: - # Object is moving - # caluculate safe speed - speed = self.calculate_safe_speed() - self.velocity_pub.publish(speed) - else: - # If object doesnt move, behaviour tree will handle - # overtaking or emergency stop was done by collision check - pass - else: - # no collisoion ahead -> publish speed limit + if self.collision_ahead is False: self.velocity_pub.publish(self.speed_limit) self.new_timer(self.control_loop_rate, loop) self.spin() diff --git a/code/planning/local_planner/src/collision_check.py b/code/planning/local_planner/src/collision_check.py index f52b6b34..aaf53330 100755 --- a/code/planning/local_planner/src/collision_check.py +++ b/code/planning/local_planner/src/collision_check.py @@ -69,13 +69,14 @@ def calculate_obstacle_speed(self, new_position: Float32): new_position (Float32): new position received from the lidar """ # Check if this is the first time the callback is called - if self.__object_last_position is None: + if self.__object_last_position is None and \ + new_position.data is not np.inf: self.__object_last_position = (rospy.get_rostime(), new_position.data) return # If speed is np.inf no car is in front - if new_position.data == np.inf: + if new_position.data is np.inf: self.__object_last_position = None return # Check if too much time has passed since last position update @@ -91,8 +92,15 @@ def calculate_obstacle_speed(self, new_position: Float32): distance = new_position.data - self.__object_last_position[1] # Speed is distance/time (m/s) - speed = distance/time_difference + relative_speed = distance/time_difference + speed = self.__current_velocity + relative_speed + self.logdebug("Relative Speed: " + str(relative_speed)) + self.logdebug("Speed: " + str(speed)) + + # Check for crash self.check_crash((distance, speed)) + self.__object_last_position = (current_time, + self._current_position[1]) def __get_current_velocity(self, data: CarlaSpeedometer): """Saves current velocity of the ego vehicle @@ -101,7 +109,6 @@ def __get_current_velocity(self, data: CarlaSpeedometer): data (CarlaSpeedometer): Message from carla with current speed """ self.__current_velocity = float(data.speed) - self.velocity_pub.publish(self.__current_velocity) def __current_position_callback(self, data: PoseStamped): """Saves current position of the ego vehicle @@ -173,16 +180,19 @@ def check_crash(self, obstacle): if distance < emergency_distance2: # Initiate emergency brake self.emergency_pub.publish(True) + self.logdebug("Emergency Brake") return # When no emergency brake is needed publish collision distance for # ACC and Behaviour tree data = Float32MultiArray(data=[collision_meter, obstacle_speed]) self.collision_pub.publish(data) + self.logdebug("Collision Distance: " + str(collision_meter)) # print(f"Safe Distance Thumb: {safe_distance2:.2f}") else: # If no collision is ahead publish np.Inf data = Float32MultiArray(data=[np.Inf, -1]) self.collision_pub(data) + self.logdebug("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 new file mode 100755 index 00000000..c0a8a620 --- /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 + + +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=Float32, + topic='/paf/' + self.role_name + '/lidar_dist', + qos_profile=1) + + self.pub_throttle = self.new_publisher( + msg_type=Float32, + topic='/paf/' + self.role_name + '/throttle', + qos_profile=1) + + self.sub_ACC = self.new_subscription( + msg_type=Float32, + topic='/paf/' + self.role_name + '/ACC', + callback=self.callback_ACC, + qos_profile=1) + + self.logdebug("DevCollisionCheck started") + self.last_position_update = None + self.simulated_speed = 12 # m/s + self.distance_to_collision = 0 + self.current_speed = 0 + self.velocity_sub = self.new_subscription( + CarlaSpeedometer, + f"/carla/{self.role_name}/Speed", + self.__get_current_velocity, + qos_profile=1) + + def callback_ACC(self, msg: Float32): + self.logdebug("ACC: " + str(msg.data)) + + def __get_current_velocity(self, msg: CarlaSpeedometer): + """ + Callback for current velocity + :param msg: + :return: + """ + self.current_speed = msg.speed + + def run(self): + """ + Control loop + :return: + """ + def loop(timer_event=None): + while self.current_speed < 15: + self.pub_throttle.publish(0.7) + self.pub_throttle.publish(0.4) + + self.pub_collision.publish(30) + time.sleep(0.3) + self.pub_collision.publish(28) + time.sleep(0.3) + self.pub_collision.publish(26) + time.sleep(0.3) + self.pub_collision.publish(24) + time.sleep(0.3) + self.pub_collision.publish(22) + time.sleep(0.3) + self.pub_collision.publish(20) + + 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() From add8190cad6d7167bac8c7c628bdadfb6909a6ff Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Fri, 15 Dec 2023 16:31:12 +0100 Subject: [PATCH 07/18] feat: test cases and test node --- .../local_planner/launch/local_planner.launch | 2 +- code/planning/local_planner/src/ACC.py | 8 +-- .../local_planner/src/collision_check.py | 67 +++++++++-------- .../src/dev_collision_publisher.py | 72 +++++++++---------- doc/07_planning/local_planning_testcase.md | 29 ++++++++ 5 files changed, 109 insertions(+), 69 deletions(-) create mode 100644 doc/07_planning/local_planning_testcase.md diff --git a/code/planning/local_planner/launch/local_planner.launch b/code/planning/local_planner/launch/local_planner.launch index 9aa9f4de..955f3d44 100644 --- a/code/planning/local_planner/launch/local_planner.launch +++ b/code/planning/local_planner/launch/local_planner.launch @@ -9,6 +9,6 @@ - + diff --git a/code/planning/local_planner/src/ACC.py b/code/planning/local_planner/src/ACC.py index 679b5566..b05b3a8e 100755 --- a/code/planning/local_planner/src/ACC.py +++ b/code/planning/local_planner/src/ACC.py @@ -22,7 +22,7 @@ def __init__(self): self.control_loop_rate = self.get_param("control_loop_rate", 1) self.current_speed = 50 / 3.6 # m/ss - self.logdebug("ACC started") + self.logerr("ACC started") # TODO: Add Subscriber for Obsdacle from Collision Check self.collision_sub = self.new_subscription( Float32MultiArray, @@ -122,7 +122,7 @@ def calculate_safe_speed(self): safe_speed = self.obstacle[1] * (self.obstacle[0] / safety_distance) - self.logdebug("Safe Speed: " + str(safe_speed)) + self.logerr("Safe Speed: " + str(safe_speed)) return safe_speed else: # If safety distance is reached, drive with same speed as @@ -130,8 +130,8 @@ def calculate_safe_speed(self): # TODO: # Incooperate overtaking -> # Communicate with decision tree about overtaking - self.logdebug("saftey distance gooood; Speed from obstacle: " + - str(self.obstacle[1])) + self.logerr("saftey distance gooood; Speed from obstacle: " + + str(self.obstacle[1])) return self.obstacle[1] def __get_current_velocity(self, data: CarlaSpeedometer): diff --git a/code/planning/local_planner/src/collision_check.py b/code/planning/local_planner/src/collision_check.py index aaf53330..a2d67f6e 100755 --- a/code/planning/local_planner/src/collision_check.py +++ b/code/planning/local_planner/src/collision_check.py @@ -1,15 +1,16 @@ #!/usr/bin/env python -import rospy +# import rospy import numpy as np # import tf.transformations import ros_compatibility as roscomp 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 +import time class CollisionCheck(CompatibleNode): @@ -24,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.logdebug("CollisionCheck started") + self.logerr("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", + Float32, # CarlaSpeedometer # f"/carla/{self.role_name}/Speed" + f"/paf/{self.role_name}/test_speed", self.__get_current_velocity, qos_profile=1) # Subscriber for current position @@ -43,7 +44,7 @@ def __init__(self): # Subscriber for lidar distance self.lidar_dist = self.new_subscription( Float32, - f"/carla/{self.role_name}/lidar_dist", + f"/carla/{self.role_name}/lidar_dist_dev", self.calculate_obstacle_speed, qos_profile=1) # Publisher for emergency stop @@ -61,54 +62,64 @@ def __init__(self): self.__object_last_position: tuple = None self._current_position: tuple = None - def calculate_obstacle_speed(self, new_position: Float32): + def calculate_obstacle_speed(self, new_dist: Float32): """Caluclate the speed of the obstacle in front of the ego vehicle based on the distance between to timestamps Args: new_position (Float32): new position received from the lidar """ + # 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 \ - new_position.data is not np.inf: - self.__object_last_position = (rospy.get_rostime(), - new_position.data) + new_dist.data is not np.inf: + self.__object_last_position = (time.time(), + new_dist.data) + self.logerr("First Position") return - # If speed is np.inf no car is in front - if new_position.data is np.inf: + # If distance is np.inf no car is in front + if new_dist.data is np.inf: self.__object_last_position = None return # Check if too much time has passed since last position update if self.__object_last_position[0] + \ - rospy.Duration(0.5) < rospy.get_rostime(): - self.__object_last_position = None + 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 = rospy.get_rostime() + current_time = time.time() time_difference = current_time-self.__object_last_position[0] # Calculate distance (in m) - distance = new_position.data - self.__object_last_position[1] + 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: " + 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.logdebug("Relative Speed: " + str(relative_speed)) - self.logdebug("Speed: " + str(speed)) + self.logerr("Speed: " + str(speed)) # Check for crash self.check_crash((distance, speed)) - self.__object_last_position = (current_time, - self._current_position[1]) + self.__object_last_position = (current_time, new_dist.data) - def __get_current_velocity(self, data: CarlaSpeedometer): + def __get_current_velocity(self, data: Float32): """Saves current velocity of the ego vehicle Args: data (CarlaSpeedometer): Message from carla with current speed """ - self.__current_velocity = float(data.speed) + self.__current_velocity = float(data.data) def __current_position_callback(self, data: PoseStamped): """Saves current position of the ego vehicle @@ -141,7 +152,7 @@ def meters_to_collision(self, obstacle_speed, distance): float: distance (in meters) until collision with obstacle in front """ return self.time_to_collision(obstacle_speed, distance) * \ - self.self.__current_velocity + self.__current_velocity def calculate_rule_of_thumb(self, emergency): """Calculates the rule of thumb as approximation @@ -174,25 +185,25 @@ def check_crash(self, obstacle): 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) + emergency_distance2 = self.calculate_rule_of_thumb(False) if collision_time > 0: if distance < emergency_distance2: # Initiate emergency brake self.emergency_pub.publish(True) - self.logdebug("Emergency Brake") + self.logerr("Emergency Brake") return # When no emergency brake is needed publish collision distance for # ACC and Behaviour tree data = Float32MultiArray(data=[collision_meter, obstacle_speed]) self.collision_pub.publish(data) - self.logdebug("Collision Distance: " + str(collision_meter)) + self.logerr("Collision Distance: " + str(collision_meter)) # print(f"Safe Distance Thumb: {safe_distance2:.2f}") else: # If no collision is ahead publish np.Inf data = Float32MultiArray(data=[np.Inf, -1]) - self.collision_pub(data) - self.logdebug("No Collision ahead") + 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 c0a8a620..f99e73b9 100755 --- a/code/planning/local_planner/src/dev_collision_publisher.py +++ b/code/planning/local_planner/src/dev_collision_publisher.py @@ -9,7 +9,6 @@ # 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 @@ -23,43 +22,56 @@ 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=Float32, - topic='/paf/' + self.role_name + '/lidar_dist', + topic='/carla/' + self.role_name + '/lidar_dist_dev', qos_profile=1) - self.pub_throttle = self.new_publisher( + self.pub_test_speed = self.new_publisher( msg_type=Float32, - topic='/paf/' + self.role_name + '/throttle', + topic='/paf/' + self.role_name + '/test_speed', qos_profile=1) - self.sub_ACC = self.new_subscription( msg_type=Float32, topic='/paf/' + self.role_name + '/ACC', callback=self.callback_ACC, qos_profile=1) - self.logdebug("DevCollisionCheck started") + self.sub_manual = self.new_subscription( + msg_type=Float32, + topic='/paf/' + self.role_name + '/manual', + callback=self.callback_manual, + qos_profile=1) + self.logerr("DevCollisionCheck started") self.last_position_update = None self.simulated_speed = 12 # m/s self.distance_to_collision = 0 self.current_speed = 0 - self.velocity_sub = self.new_subscription( - CarlaSpeedometer, - f"/carla/{self.role_name}/Speed", - self.__get_current_velocity, - qos_profile=1) + self.manual_start = True + self.acc_activated = False - def callback_ACC(self, msg: Float32): - self.logdebug("ACC: " + str(msg.data)) + 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) + self.pub_lidar.publish(Float32(data=25)) + time.sleep(0.2) + self.pub_lidar.publish(Float32(data=22)) + 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 __get_current_velocity(self, msg: CarlaSpeedometer): - """ - Callback for current velocity - :param msg: - :return: - """ - self.current_speed = msg.speed + 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): """ @@ -67,22 +79,10 @@ def run(self): :return: """ def loop(timer_event=None): - while self.current_speed < 15: - self.pub_throttle.publish(0.7) - self.pub_throttle.publish(0.4) - - self.pub_collision.publish(30) - time.sleep(0.3) - self.pub_collision.publish(28) - time.sleep(0.3) - self.pub_collision.publish(26) - time.sleep(0.3) - self.pub_collision.publish(24) - time.sleep(0.3) - self.pub_collision.publish(22) - time.sleep(0.3) - self.pub_collision.publish(20) - + if self.acc_activated is False: + self.pub_test_speed.publish(Float32(data=13.8889)) + else: + self.pub_test_speed.publish(Float32(data=self.current_speed)) self.new_timer(self.control_loop_rate, loop) self.spin() diff --git a/doc/07_planning/local_planning_testcase.md b/doc/07_planning/local_planning_testcase.md new file mode 100644 index 00000000..68bcc2a7 --- /dev/null +++ b/doc/07_planning/local_planning_testcase.md @@ -0,0 +1,29 @@ +# Preplanning + +**Summary:** Definition of test cases for collision check and local planning + +--- + +## Author + +Samuel Kühnel + +## Date + +15.12.2023 + +## Test cases + +### Car with same speed in front + +A car drives with the same speed and the safety distance is kept. + +**Speed**: 50 km/h +**Distance**: 25 m (Rule of thumb) + +### Car with lower speed in front + +A car with lower speed is in front -> decreasing distance + +**Speed**: 50 km/h +**Distance**: 25 m (Rule of thumb) -> decreases as car in front is slower From d9416aa3419ce66d09efad8838c2b0d39b85a2cb Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Sat, 16 Dec 2023 16:37:38 +0100 Subject: [PATCH 08/18] feat: Testing done --- build/docker-compose.yml | 4 +- code/planning/local_planner/src/ACC.py | 22 +- .../local_planner/src/collision_check.py | 39 +- .../src/dev_collision_publisher.py | 27 +- .../Leaderboard-2/capture_sensor_data.py | 431 ------------------ 5 files changed, 50 insertions(+), 473 deletions(-) delete mode 100644 doc/03_research/Leaderboard-2/capture_sensor_data.py diff --git a/build/docker-compose.yml b/build/docker-compose.yml index 47e13068..a68e97c4 100644 --- a/build/docker-compose.yml +++ b/build/docker-compose.yml @@ -57,8 +57,8 @@ 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/planning/local_planner/src/ACC.py b/code/planning/local_planner/src/ACC.py index b05b3a8e..befb6ca8 100755 --- a/code/planning/local_planner/src/ACC.py +++ b/code/planning/local_planner/src/ACC.py @@ -9,6 +9,7 @@ from nav_msgs.msg import Path # from std_msgs.msg import String from std_msgs.msg import Float32MultiArray, Float32 +from collision_check import CollisionCheck class ACC(CompatibleNode): @@ -32,8 +33,8 @@ def __init__(self): # Get current speed self.velocity_sub: Subscriber = self.new_subscription( - CarlaSpeedometer, - f"/carla/{self.role_name}/Speed", + Float32, + f"/paf/{self.role_name}/test_speed", self.__get_current_velocity, qos_profile=1) @@ -77,7 +78,7 @@ def __init__(self): # Distance and speed from possible collsion object self.obstacle: tuple = None # Current speed limit - self.speed_limit: float = np.Inf + self.speed_limit: float = None # m/s def __get_collision(self, data: Float32MultiArray): """Check if collision is ahead @@ -86,9 +87,10 @@ def __get_collision(self, data: Float32MultiArray): data (Float32MultiArray): Distance and speed from possible collsion object """ - if data.data[1] == np.Inf: + 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 @@ -108,10 +110,10 @@ def calculate_safe_speed(self): return None if self.obstacle is None: return None - # 1s * m/s = reaction distance - reaction_distance = self.__current_velocity - safety_distance = reaction_distance + \ - (self.__current_velocity * 0.36)**2 + # 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 @@ -130,7 +132,7 @@ def calculate_safe_speed(self): # TODO: # Incooperate overtaking -> # Communicate with decision tree about overtaking - self.logerr("saftey distance gooood; Speed from obstacle: " + + self.logerr("saftey distance good; Speed from obstacle: " + str(self.obstacle[1])) return self.obstacle[1] @@ -140,7 +142,7 @@ def __get_current_velocity(self, data: CarlaSpeedometer): Args: data (CarlaSpeedometer): _description_ """ - self.__current_velocity = float(data.speed) + self.__current_velocity = float(data.data) 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 a2d67f6e..0aa3a9f1 100755 --- a/code/planning/local_planner/src/collision_check.py +++ b/code/planning/local_planner/src/collision_check.py @@ -23,7 +23,7 @@ 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.logerr("CollisionCheck started") @@ -76,15 +76,18 @@ def calculate_obstacle_speed(self, new_dist: Float32): # 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 \ - new_dist.data is not np.inf: + 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 - if new_dist.data is np.inf: + if np.isinf(new_dist.data): 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] + \ @@ -102,7 +105,7 @@ def calculate_obstacle_speed(self, new_dist: Float32): # Speed is distance/time (m/s) self.logerr("Time Difference: " + str(time_difference)) - self.logerr("Distance: " + str(distance)) + 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)) @@ -110,7 +113,7 @@ def calculate_obstacle_speed(self, new_dist: Float32): self.logerr("Speed: " + str(speed)) # Check for crash - self.check_crash((distance, speed)) + self.check_crash((new_dist.data, speed)) self.__object_last_position = (current_time, new_dist.data) def __get_current_velocity(self, data: Float32): @@ -139,7 +142,9 @@ def time_to_collision(self, obstacle_speed, distance): Returns: float: Time until collision with obstacle in front """ - return distance / (self.current_speed - obstacle_speed) + if (self.__current_velocity - obstacle_speed) == 0: + return -1 + return distance / (self.__current_velocity - obstacle_speed) def meters_to_collision(self, obstacle_speed, distance): """Calculates the meters until collision with the obstacle in front @@ -154,7 +159,8 @@ def meters_to_collision(self, obstacle_speed, distance): return self.time_to_collision(obstacle_speed, distance) * \ self.__current_velocity - def calculate_rule_of_thumb(self, emergency): + @staticmethod + def calculate_rule_of_thumb(emergency, speed): """Calculates the rule of thumb as approximation for the braking distance @@ -164,8 +170,8 @@ def calculate_rule_of_thumb(self, emergency): Returns: float: distance calculated with rule of thumb """ - 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: @@ -182,11 +188,12 @@ def check_crash(self, obstacle): distance, obstacle_speed = obstacle collision_time = self.time_to_collision(obstacle_speed, distance) - collision_meter = self.meters_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(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 @@ -195,13 +202,13 @@ def check_crash(self, obstacle): return # When no emergency brake is needed publish collision distance for # ACC and Behaviour tree - data = Float32MultiArray(data=[collision_meter, obstacle_speed]) + data = Float32MultiArray(data=[distance, obstacle_speed]) self.collision_pub.publish(data) - self.logerr("Collision Distance: " + str(collision_meter)) + 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, -1]) + data = Float32MultiArray(data=[np.Inf, obstacle_speed]) self.collision_pub.publish(data) self.logerr("No Collision ahead") diff --git a/code/planning/local_planner/src/dev_collision_publisher.py b/code/planning/local_planner/src/dev_collision_publisher.py index f99e73b9..fe8c3368 100755 --- a/code/planning/local_planner/src/dev_collision_publisher.py +++ b/code/planning/local_planner/src/dev_collision_publisher.py @@ -10,6 +10,7 @@ # from std_msgs.msg import String from std_msgs.msg import Float32 import time +# import numpy as np class DevCollisionCheck(CompatibleNode): @@ -34,7 +35,7 @@ def __init__(self): qos_profile=1) self.sub_ACC = self.new_subscription( msg_type=Float32, - topic='/paf/' + self.role_name + '/ACC', + topic='/paf/' + self.role_name + '/acc_velocity', callback=self.callback_ACC, qos_profile=1) @@ -59,18 +60,18 @@ def callback_manual(self, msg: Float32): time.sleep(0.2) self.pub_lidar.publish(Float32(data=25)) time.sleep(0.2) - self.pub_lidar.publish(Float32(data=22)) - 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)) + self.pub_lidar.publish(Float32(data=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.logerr("Timestamp: " + time.time().__str__()) + # self.logerr("ACC: " + str(msg.data)) self.current_speed = msg.data def run(self): @@ -79,10 +80,8 @@ def run(self): :return: """ def loop(timer_event=None): - if self.acc_activated is False: - self.pub_test_speed.publish(Float32(data=13.8889)) - else: - self.pub_test_speed.publish(Float32(data=self.current_speed)) + self.pub_test_speed.publish(Float32(data=13.8889)) + self.new_timer(self.control_loop_rate, loop) self.spin() 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.') From f93dadfdea042e91056123e1b5604074cb74c7d9 Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Sun, 17 Dec 2023 13:49:22 +0100 Subject: [PATCH 09/18] 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) From 7f9c51f75baa19d27d4a03ff2339c1d318e03c72 Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Sun, 17 Dec 2023 18:22:36 +0100 Subject: [PATCH 10/18] fix: linter error --- code/planning/local_planner/src/collision_check.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/code/planning/local_planner/src/collision_check.py b/code/planning/local_planner/src/collision_check.py index db4e6add..082e257e 100755 --- a/code/planning/local_planner/src/collision_check.py +++ b/code/planning/local_planner/src/collision_check.py @@ -42,9 +42,10 @@ def __init__(self): callback=self.__current_position_callback, qos_profile=1) # Subscriber for lidar distance + # TODO: Change to real lidar distance self.lidar_dist = self.new_subscription( Float32, - f"/carla/{self.role_name}/lidar_dist_dev", # TODO: Change to lidar topic + f"/carla/{self.role_name}/lidar_dist_dev", self.calculate_obstacle_speed, qos_profile=1) # Publisher for emergency stop From c17713e6f7b44518fdca1abb68cdd94a380babbc Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Sun, 17 Dec 2023 18:27:38 +0100 Subject: [PATCH 11/18] feat: no ACC when emergency --- code/planning/local_planner/src/ACC.py | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/code/planning/local_planner/src/ACC.py b/code/planning/local_planner/src/ACC.py index cde7195a..445ec998 100755 --- a/code/planning/local_planner/src/ACC.py +++ b/code/planning/local_planner/src/ACC.py @@ -8,7 +8,7 @@ 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 std_msgs.msg import Float32MultiArray, Float32, Bool from collision_check import CollisionCheck @@ -55,6 +55,12 @@ def __init__(self): # Get current position to determine current speed limit self.current_pos_sub: Subscriber = self.new_subscription( msg_type=PoseStamped, + topic="/paf/" + self.role_name + "/emergency", + callback=self.emergency_callback, + qos_profile=1) + + self.emergency_sub: Subscriber = self.new_subscription( + msg_type=Bool, topic="/paf/" + self.role_name + "/current_pos", callback=self.__current_position_callback, qos_profile=1) @@ -80,6 +86,16 @@ def __init__(self): # Current speed limit self.speed_limit: float = None # m/s + def emergency_callback(self, data: Bool): + """Callback for emergency stop + Turn of ACC when emergency stop is triggered + + Args: + data (Bool): Emergency stop + """ + if data.data is True: + self.collision_ahead = True + def __get_collision(self, data: Float32MultiArray): """Check if collision is ahead From 37fbe5ec5909a19c5f09076154c2683a3a08937d Mon Sep 17 00:00:00 2001 From: samuelkuehnel <51356601+samuelkuehnel@users.noreply.github.com> Date: Sun, 17 Dec 2023 18:35:38 +0100 Subject: [PATCH 12/18] fix: remove unused subscriber --- .../local_planner/src/collision_check.py | 16 ---------------- 1 file changed, 16 deletions(-) diff --git a/code/planning/local_planner/src/collision_check.py b/code/planning/local_planner/src/collision_check.py index 0aa3a9f1..c2916c19 100755 --- a/code/planning/local_planner/src/collision_check.py +++ b/code/planning/local_planner/src/collision_check.py @@ -5,7 +5,6 @@ import ros_compatibility as roscomp from ros_compatibility.node import CompatibleNode from rospy import Subscriber -from geometry_msgs.msg import PoseStamped # from carla_msgs.msg import CarlaSpeedometer # , CarlaWorldInfo # from std_msgs.msg import String from std_msgs.msg import Float32, Float32MultiArray @@ -35,12 +34,6 @@ def __init__(self): f"/paf/{self.role_name}/test_speed", self.__get_current_velocity, qos_profile=1) - # Subscriber for current position - self.current_pos_sub: Subscriber = self.new_subscription( - msg_type=PoseStamped, - topic="/paf/" + self.role_name + "/current_pos", - callback=self.__current_position_callback, - qos_profile=1) # Subscriber for lidar distance self.lidar_dist = self.new_subscription( Float32, @@ -60,7 +53,6 @@ def __init__(self): # Variables to save vehicle data self.__current_velocity: float = None self.__object_last_position: tuple = None - self._current_position: tuple = None def calculate_obstacle_speed(self, new_dist: Float32): """Caluclate the speed of the obstacle in front of the ego vehicle @@ -124,14 +116,6 @@ def __get_current_velocity(self, data: Float32): """ self.__current_velocity = float(data.data) - def __current_position_callback(self, data: PoseStamped): - """Saves current position of the ego vehicle - - Args: - data (PoseStamped): Message from Perception with current position - """ - self._current_position = (data.pose.position.x, data.pose.position.y) - def time_to_collision(self, obstacle_speed, distance): """calculates the time to collision with the obstacle in front From e394ad002527a906ff63b4e2613ee3d5bb5ac2f6 Mon Sep 17 00:00:00 2001 From: samuelkuehnel <51356601+samuelkuehnel@users.noreply.github.com> Date: Sun, 17 Dec 2023 19:36:37 +0100 Subject: [PATCH 13/18] doc: ACC documentation added --- code/planning/local_planner/src/ACC.py | 2 +- doc/07_planning/local_planning.md | 103 +++++++++++++++++++++++++ 2 files changed, 104 insertions(+), 1 deletion(-) create mode 100644 doc/07_planning/local_planning.md diff --git a/code/planning/local_planner/src/ACC.py b/code/planning/local_planner/src/ACC.py index 445ec998..44fad046 100755 --- a/code/planning/local_planner/src/ACC.py +++ b/code/planning/local_planner/src/ACC.py @@ -58,7 +58,7 @@ def __init__(self): topic="/paf/" + self.role_name + "/emergency", callback=self.emergency_callback, qos_profile=1) - + self.emergency_sub: Subscriber = self.new_subscription( msg_type=Bool, topic="/paf/" + self.role_name + "/current_pos", diff --git a/doc/07_planning/local_planning.md b/doc/07_planning/local_planning.md new file mode 100644 index 00000000..3027498f --- /dev/null +++ b/doc/07_planning/local_planning.md @@ -0,0 +1,103 @@ +# 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. From f0f208d882927b8542db05c793a0c8c2e4ba1f25 Mon Sep 17 00:00:00 2001 From: samuelkuehnel <51356601+samuelkuehnel@users.noreply.github.com> Date: Sun, 17 Dec 2023 19:38:51 +0100 Subject: [PATCH 14/18] Update local_planning.md --- doc/07_planning/local_planning.md | 1 + 1 file changed, 1 insertion(+) diff --git a/doc/07_planning/local_planning.md b/doc/07_planning/local_planning.md index 3027498f..cefa0a5e 100644 --- a/doc/07_planning/local_planning.md +++ b/doc/07_planning/local_planning.md @@ -63,6 +63,7 @@ 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} $$ From 868885e897534cf1270dc0c04a7b7e1e98002a46 Mon Sep 17 00:00:00 2001 From: samuelkuehnel <51356601+samuelkuehnel@users.noreply.github.com> Date: Sun, 17 Dec 2023 19:39:59 +0100 Subject: [PATCH 15/18] doc: removed old file --- doc/07_planning/local_planning_testcase.md | 29 ---------------------- 1 file changed, 29 deletions(-) delete mode 100644 doc/07_planning/local_planning_testcase.md diff --git a/doc/07_planning/local_planning_testcase.md b/doc/07_planning/local_planning_testcase.md deleted file mode 100644 index 68bcc2a7..00000000 --- a/doc/07_planning/local_planning_testcase.md +++ /dev/null @@ -1,29 +0,0 @@ -# Preplanning - -**Summary:** Definition of test cases for collision check and local planning - ---- - -## Author - -Samuel Kühnel - -## Date - -15.12.2023 - -## Test cases - -### Car with same speed in front - -A car drives with the same speed and the safety distance is kept. - -**Speed**: 50 km/h -**Distance**: 25 m (Rule of thumb) - -### Car with lower speed in front - -A car with lower speed is in front -> decreasing distance - -**Speed**: 50 km/h -**Distance**: 25 m (Rule of thumb) -> decreases as car in front is slower From 0a86002a6ab9e6853e9aa9c07779806f45119fd1 Mon Sep 17 00:00:00 2001 From: samuelkuehnel <51356601+samuelkuehnel@users.noreply.github.com> Date: Tue, 19 Dec 2023 01:17:35 +0100 Subject: [PATCH 16/18] fix: Typos and partially implemented Feedback from sprint review --- .../launch/global_planner.launch | 4 +- code/planning/local_planner/src/ACC.py | 66 ++++++++----------- .../local_planner/src/collision_check.py | 8 ++- 3 files changed, 37 insertions(+), 41 deletions(-) diff --git a/code/planning/global_planner/launch/global_planner.launch b/code/planning/global_planner/launch/global_planner.launch index 8d65c20c..cdb6dec9 100644 --- a/code/planning/global_planner/launch/global_planner.launch +++ b/code/planning/global_planner/launch/global_planner.launch @@ -1,12 +1,12 @@ - + diff --git a/code/planning/local_planner/src/ACC.py b/code/planning/local_planner/src/ACC.py index 44fad046..2e72554e 100755 --- a/code/planning/local_planner/src/ACC.py +++ b/code/planning/local_planner/src/ACC.py @@ -10,6 +10,8 @@ # from std_msgs.msg import String from std_msgs.msg import Float32MultiArray, Float32, Bool from collision_check import CollisionCheck +import time +from perception.msg import MinDistance class ACC(CompatibleNode): @@ -23,14 +25,6 @@ def __init__(self): self.control_loop_rate = self.get_param("control_loop_rate", 1) self.current_speed = 50 / 3.6 # m/ss - self.loginfo("ACC started") - # TODO: Add Subscriber for Obsdacle from Collision Check - self.collision_sub = self.new_subscription( - Float32MultiArray, - f"/paf/{self.role_name}/collision", - self.__get_collision, - qos_profile=1) - # Get current speed self.velocity_sub: Subscriber = self.new_subscription( CarlaSpeedometer, @@ -38,6 +32,13 @@ def __init__(self): 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"/carla/{self.role_name}/LIDAR_range", + self._set_distance, + qos_profile=1) # Get initial set of speed limits self.speed_limit_OD_sub: Subscriber = self.new_subscription( Float32MultiArray, @@ -52,20 +53,17 @@ def __init__(self): self.__set_trajectory, qos_profile=1) - # Get current position to determine current speed limit - self.current_pos_sub: Subscriber = self.new_subscription( - msg_type=PoseStamped, - topic="/paf/" + self.role_name + "/emergency", - callback=self.emergency_callback, - qos_profile=1) - self.emergency_sub: Subscriber = self.new_subscription( msg_type=Bool, topic="/paf/" + self.role_name + "/current_pos", callback=self.__current_position_callback, qos_profile=1) - - # Publish desiored speed to acting + 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", @@ -82,37 +80,29 @@ def __init__(self): # Is an obstacle ahead where we would collide with? self.collision_ahead: bool = False # Distance and speed from possible collsion object - self.obstacle: tuple = None + self.obstacle_speed: tuple = None + # Obstalce distance + self.obstacle_distance = None # Current speed limit self.speed_limit: float = None # m/s - def emergency_callback(self, data: Bool): - """Callback for emergency stop - Turn of ACC when emergency stop is triggered + def _set_distance(self, data: MinDistance): + """Get min distance to object in front from perception Args: - data (Bool): Emergency stop + data (MinDistance): Minimum Distance from LIDAR """ - if data.data is True: - self.collision_ahead = True + self.obstacle_distance = data.distance - def __get_collision(self, data: Float32MultiArray): - """Check if collision is ahead + 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 (Float32MultiArray): Distance and speed from possible - collsion object + data (Float32): Speed from obstacle in front """ - if np.isinf(data.data[0]): - # No collision ahead - self.collision_ahead = False - else: - # Collision ahead - self.collision_ahead = True - self.obstacle = (data.data[0], data.data[1]) - target_speed = self.calculate_safe_speed() - if target_speed is not None: - self.velocity_pub.publish(target_speed) + self.obstacle_speed = (time.time(), data.data) def calculate_safe_speed(self): """calculates the speed to meet the desired distance to the object diff --git a/code/planning/local_planner/src/collision_check.py b/code/planning/local_planner/src/collision_check.py index 7af8ca07..606a60f9 100755 --- a/code/planning/local_planner/src/collision_check.py +++ b/code/planning/local_planner/src/collision_check.py @@ -51,6 +51,11 @@ def __init__(self): 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.rolename}/cc_speed", + qos_profile=1) # Variables to save vehicle data self.__current_velocity: float = None self.__object_last_position: tuple = None @@ -94,7 +99,8 @@ def calculate_obstacle_speed(self, new_dist: Float32): # Speed is distance/time (m/s) relative_speed = distance/time_difference speed = self.__current_velocity + relative_speed - + # Publish distance and speed to ACC for permanent distance check + self.speed_publisher(Float32(data=speed)) # Check for crash self.check_crash((new_dist.data, speed)) self.__object_last_position = (current_time, new_dist.data) From 30007b5c003f886517a827d0eef8e0538bdeace5 Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Tue, 19 Dec 2023 12:45:13 +0100 Subject: [PATCH 17/18] fix: implemented Feedback from review ACC now permanently checking the distance --- .../local_planner/launch/local_planner.launch | 2 +- code/planning/local_planner/src/ACC.py | 72 +++++++++---------- .../local_planner/src/collision_check.py | 31 ++++---- 3 files changed, 47 insertions(+), 58 deletions(-) diff --git a/code/planning/local_planner/launch/local_planner.launch b/code/planning/local_planner/launch/local_planner.launch index a27956cd..c75bc24e 100644 --- a/code/planning/local_planner/launch/local_planner.launch +++ b/code/planning/local_planner/launch/local_planner.launch @@ -5,7 +5,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 index 18ab7c24..d13357fa 100755 --- a/code/planning/local_planner/src/ACC.py +++ b/code/planning/local_planner/src/ACC.py @@ -7,7 +7,7 @@ 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, Bool +from std_msgs.msg import Float32MultiArray, Float32 from collision_check import CollisionCheck import time from perception.msg import MinDistance @@ -53,7 +53,7 @@ def __init__(self): qos_profile=1) self.emergency_sub: Subscriber = self.new_subscription( - msg_type=Bool, + msg_type=PoseStamped, topic="/paf/" + self.role_name + "/current_pos", callback=self.__current_position_callback, qos_profile=1) @@ -140,7 +140,6 @@ def __current_position_callback(self, data: PoseStamped): 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) @@ -182,10 +181,12 @@ def loop(timer_event=None): # 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() diff --git a/code/planning/local_planner/src/collision_check.py b/code/planning/local_planner/src/collision_check.py index 86388e05..5fe0d95a 100755 --- a/code/planning/local_planner/src/collision_check.py +++ b/code/planning/local_planner/src/collision_check.py @@ -55,7 +55,7 @@ def __init__(self): # Approx speed publisher for ACC self.speed_publisher = self.new_publisher( Float32, - f"/paf/{self.rolename}/cc_speed", + f"/paf/{self.role_name}/cc_speed", qos_profile=1) # Variables to save vehicle data self.__current_velocity: float = None @@ -99,7 +99,7 @@ def calculate_obstacle_speed(self, new_dist: MinDistance): relative_speed = distance/time_difference speed = self.__current_velocity + relative_speed # Publish speed to ACC for permanent distance check - self.speed_publisher(Float32(data=speed)) + 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) diff --git a/code/planning/local_planner/src/dev_collision_publisher.py b/code/planning/local_planner/src/dev_collision_publisher.py index 65fb1c2e..9bbd267a 100755 --- a/code/planning/local_planner/src/dev_collision_publisher.py +++ b/code/planning/local_planner/src/dev_collision_publisher.py @@ -9,7 +9,9 @@ # 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 @@ -25,13 +27,13 @@ def __init__(self): self.control_loop_rate = self.get_param("control_loop_rate", 1) self.pub_lidar = self.new_publisher( - msg_type=Float32, - topic='/carla/' + self.role_name + '/lidar_dist_dev', + msg_type=MinDistance, + topic=f'/paf/{self.role_name}/Center/min_distance', qos_profile=1) self.pub_test_speed = self.new_publisher( - msg_type=Float32, - topic='/paf/' + self.role_name + '/test_speed', + msg_type=CarlaSpeedometer, + topic='/carla/' + self.role_name + '/test_speed', qos_profile=1) self.sub_ACC = self.new_subscription( msg_type=Float32, @@ -55,11 +57,11 @@ def __init__(self): def callback_manual(self, msg: Float32): if self.manual_start: self.manual_start = False - self.pub_lidar.publish(Float32(data=25)) + self.pub_lidar.publish(MinDistance(distance=25)) time.sleep(0.2) - self.pub_lidar.publish(Float32(data=25)) + self.pub_lidar.publish(MinDistance(distance=25)) time.sleep(0.2) - self.pub_lidar.publish(Float32(data=24)) + self.pub_lidar.publish(MinDistance(distance=24)) # time.sleep(0.2) # self.pub_lidar.publish(Float32(data=20)) # time.sleep(0.2) @@ -69,8 +71,8 @@ def callback_manual(self, msg: Float32): def callback_ACC(self, msg: Float32): self.acc_activated = True - # self.logerr("Timestamp: " + time.time().__str__()) - # self.logerr("ACC: " + str(msg.data)) + self.logerr("Timestamp: " + time.time().__str__()) + self.logerr("ACC: " + str(msg.data)) self.current_speed = msg.data def run(self): @@ -79,7 +81,7 @@ def run(self): :return: """ def loop(timer_event=None): - self.pub_test_speed.publish(Float32(data=13.8889)) + self.pub_test_speed.publish(CarlaSpeedometer(speed=13.8889)) self.new_timer(self.control_loop_rate, loop) self.spin()