diff --git a/code/perception/src/vision_node.py b/code/perception/src/vision_node.py index 4897391c..bf15a011 100755 --- a/code/perception/src/vision_node.py +++ b/code/perception/src/vision_node.py @@ -368,6 +368,7 @@ def predict_ultralytics(self, image): else: obj_dist1 = (np.inf, np.inf, np.inf) + obj_dist3 = (np.inf, np.inf, np.inf) abs_distance = np.inf """distance_output.append(float(cls)) distance_output.append(float(abs_distance)) diff --git a/code/planning/src/behavior_agent/behaviours/overtake.py b/code/planning/src/behavior_agent/behaviours/overtake.py index 36113781..bf0941be 100644 --- a/code/planning/src/behavior_agent/behaviours/overtake.py +++ b/code/planning/src/behavior_agent/behaviours/overtake.py @@ -15,6 +15,10 @@ def convert_to_ms(speed): return speed / 3.6 +# Varaible to determine if overtake is currently exec +OVERTAKE_EXECUTING = False + + class Approach(py_trees.behaviour.Behaviour): """ This behaviour is executed when the ego vehicle is in close proximity of @@ -62,13 +66,7 @@ def initialise(self): rospy.loginfo("Approaching Overtake") self.ot_distance = 30 - self.ot_option = 1 # self.blackboard.get("paf/hero/...") - if self.ot_option == 0: - self.clear_distance = 15 - self.side = "LIDAR_range_rear_left" - elif self.ot_option == 1: - self.clear_distance = 30 - self.side = "LIDAR_range" + self.clear_distance = 30 def update(self): """ @@ -86,27 +84,28 @@ def update(self): object or entered the process. py_trees.common.Status.FAILURE, """ + global OVERTAKE_EXECUTING # Update distance to collision object _dis = self.blackboard.get("/paf/hero/collision") if _dis is not None: self.ot_distance = _dis.data[0] rospy.loginfo(f"Overtake distance: {self.ot_distance}") + OVERTAKE_EXECUTING = self.ot_distance if np.isinf(self.ot_distance): - rospy.loginfo("Abort Overtake") + rospy.loginfo("OvertakeApproach: Abort") return py_trees.common.Status.FAILURE # slow down before overtake if blocked - if self.ot_distance < 15.0: - distance_lidar = self.blackboard. \ - get(f"/carla/hero/{self.side}") - if distance_lidar is not None: - distance_lidar = distance_lidar.data + if self.ot_distance < 20.0: + data = self.blackboard.get("/paf/hero/oncoming") + if data is not None: + distance_oncoming = data.data else: - distance_lidar = None + distance_oncoming = 35 - if distance_lidar is not None and \ - distance_lidar > self.clear_distance: + if distance_oncoming is not None and \ + distance_oncoming > self.clear_distance: rospy.loginfo("Overtake is free not slowing down!") self.curr_behavior_pub.publish(bs.ot_app_free.name) return py_trees.common.Status.SUCCESS @@ -122,17 +121,17 @@ def update(self): rospy.logwarn("no speedometer connected") return py_trees.common.Status.RUNNING - if self.ot_distance > 15.0: + if self.ot_distance > 20.0: # too far rospy.loginfo("still approaching") return py_trees.common.Status.RUNNING elif speed < convert_to_ms(2.0) and \ - self.ot_distance < 8.0: + self.ot_distance < 6.0: # stopped rospy.loginfo("stopped") return py_trees.common.Status.SUCCESS else: - rospy.logerr("Overtake Approach: Default Case") + # still approaching return py_trees.common.Status.RUNNING def terminate(self, new_status): @@ -214,34 +213,29 @@ def update(self): """ # Update distance to collison and distance for clear - self.ot_option = 1 # self.blackboard.get("paf/hero/...") - if self.ot_option == 0: - distance_lidar = self.blackboard. \ - get("/carla/hero/LIDAR_range_rear_left") - clear_distance = 15 - elif self.ot_option == 1: - distance_lidar = self.blackboard. \ - get("/carla/hero/LIDAR_range") - clear_distance = 30 - else: - distance_lidar = None - + clear_distance = 30 obstacle_msg = self.blackboard.get("/paf/hero/collision") if obstacle_msg is None: rospy.logerr("No OBSTACLE") return py_trees.common.Status.FAILURE - if distance_lidar is not None: - collision_distance = distance_lidar.data - if collision_distance > clear_distance: + data = self.blackboard.get("/paf/hero/oncoming") + if data is not None: + distance_oncoming = data.data + else: + distance_oncoming = 35 + + if distance_oncoming is not None: + if distance_oncoming > clear_distance: rospy.loginfo("Overtake is free!") self.curr_behavior_pub.publish(bs.ot_wait_free.name) return py_trees.common.Status.SUCCESS else: - rospy.loginfo("Overtake still blocked") + rospy.loginfo(f"Overtake still blocked: {distance_oncoming}") self.curr_behavior_pub.publish(bs.ot_wait_stopped.name) - return py_trees.commom.Status.RUNNING + return py_trees.common.Status.RUNNING elif obstacle_msg.data[0] == np.inf: + rospy.loginf("No OBSTACLE") return py_trees.common.Status.FAILURE else: rospy.loginfo("No Lidar Distance") @@ -332,11 +326,11 @@ def update(self): rospy.loginfo("Overtake: Slowing down") return py_trees.common.Status.RUNNING else: - rospy.loginfo("OvertakeEnter: Abort ") + rospy.loginfo("OvertakeEnter: Abort") return py_trees.common.Status.FAILURE else: - rospy.loginfo("Overtake: Bigger Failure") - return py_trees.common.Status.FAILURE + rospy.loginfo("Overtake: Waiting for status update") + return py_trees.common.Status.RUNNING # Currently not in use # Can be used to check if we can go back to the original lane @@ -396,11 +390,11 @@ def initialise(self): Any initialisation you need before putting your behaviour to work. This prints a state status message and publishes the behavior """ - rospy.loginfo("Leave Overtake") self.curr_behavior_pub.publish(bs.ot_leave.name) data = self.blackboard.get("/paf/hero/current_pos") self.first_pos = np.array([data.pose.position.x, data.pose.position.y]) + rospy.loginfo(f"Leave Overtake: {self.first_pos}") return True def update(self): @@ -414,11 +408,13 @@ def update(self): Abort this subtree :return: py_trees.common.Status.FAILURE, to exit this subtree """ + global OVERTAKE_EXECUTING data = self.blackboard.get("/paf/hero/current_pos") self.current_pos = np.array([data.pose.position.x, data.pose.position.y]) distance = np.linalg.norm(self.first_pos - self.current_pos) - if distance > 10: + if distance > OVERTAKE_EXECUTING: + rospy.loginfo(f"Left Overtake: {self.current_pos}") return py_trees.common.Status.FAILURE else: return py_trees.common.Status.RUNNING diff --git a/code/planning/src/behavior_agent/behaviours/road_features.py b/code/planning/src/behavior_agent/behaviours/road_features.py index c981ad88..a00d566b 100755 --- a/code/planning/src/behavior_agent/behaviours/road_features.py +++ b/code/planning/src/behavior_agent/behaviours/road_features.py @@ -5,8 +5,6 @@ import numpy as np from scipy.spatial.transform import Rotation import rospy - - """ Source: https://github.com/ll7/psaf2 """ @@ -264,7 +262,7 @@ def update(self): if np.linalg.norm(pos_moved_in_x_direction - current_position) < 1: # current collision is not near trajectory lane - self.logerr("Obstacle is not near trajectory lane") + rospy.logerr("Obstacle is not near trajectory lane") return py_trees.common.Status.FAILURE if obstacle_speed < 2 and obstacle_distance < 30: diff --git a/code/planning/src/behavior_agent/behaviours/topics2blackboard.py b/code/planning/src/behavior_agent/behaviours/topics2blackboard.py index ae7754dc..d26a717d 100755 --- a/code/planning/src/behavior_agent/behaviours/topics2blackboard.py +++ b/code/planning/src/behavior_agent/behaviours/topics2blackboard.py @@ -38,8 +38,6 @@ def create_node(role_name): 'clearing-policy': py_trees.common.ClearingPolicy.NEVER}, {'name': f"/paf/{role_name}/max_velocity", 'msg': Float32, 'clearing-policy': py_trees.common.ClearingPolicy.NEVER}, - {'name': f"/carla/{role_name}/LIDAR_range", 'msg': Float32, - 'clearing-policy': py_trees.common.ClearingPolicy.NEVER}, {'name': f"/paf/{role_name}/speed_limit", 'msg': Float32, 'clearing-policy': py_trees.common.ClearingPolicy.NEVER}, {'name': f"/paf/{role_name}/lane_change_distance", 'msg': LaneChange, @@ -52,6 +50,8 @@ def create_node(role_name): 'clearing-policy': py_trees.common.ClearingPolicy.NEVER}, {'name': f"/paf/{role_name}/overtake_success", 'msg': Float32, 'clearing-policy': py_trees.common.ClearingPolicy.NEVER}, + {'name': f"/paf/{role_name}/oncoming", 'msg': Float32, + 'clearing-policy': py_trees.common.ClearingPolicy.NEVER}, ] topics2blackboard = py_trees.composites.Parallel("Topics to Blackboard") diff --git a/code/planning/src/local_planner/ACC.py b/code/planning/src/local_planner/ACC.py index 60f43123..e1ac88be 100755 --- a/code/planning/src/local_planner/ACC.py +++ b/code/planning/src/local_planner/ACC.py @@ -173,7 +173,7 @@ def loop(timer_event=None): # https://encyclopediaofmath.org/index.php?title=Linear_interpolation safe_speed = self.obstacle_speed * \ (self.obstacle_distance / safety_distance) - lerp_factor = 0.1 + lerp_factor = 0.2 safe_speed = (1 - lerp_factor) * self.__current_velocity +\ lerp_factor * safe_speed if safe_speed < 1.0: @@ -187,7 +187,11 @@ def loop(timer_event=None): # self.obstacle_speed = 0 # self.logerr("ACC: my speed: " + # str(self.__current_velocity)) - self.velocity_pub.publish(self.__current_velocity) + if self.__current_velocity < 1.0: + safe_speed = 0 + else: + safe_speed = self.__current_velocity + self.velocity_pub.publish(safe_speed) elif self.speed_limit is not None: # If we have no obstacle, we want to drive with the current diff --git a/code/planning/src/local_planner/collision_check.py b/code/planning/src/local_planner/collision_check.py index b7039f8d..85540e21 100755 --- a/code/planning/src/local_planner/collision_check.py +++ b/code/planning/src/local_planner/collision_check.py @@ -38,7 +38,7 @@ def __init__(self): self.lidar_dist = self.new_subscription( Float32MultiArray, f"/paf/{self.role_name}/Center/object_distance", - self.__set_distance, + self.__set_all_distances, qos_profile=1) # Publisher for emergency stop self.emergency_pub = self.new_publisher( @@ -50,6 +50,10 @@ def __init__(self): Float32MultiArray, f"/paf/{self.role_name}/collision", qos_profile=1) + self.oncoming_pub = self.new_publisher( + Float32, + f"/paf/{self.role_name}/oncoming", + qos_profile=1) # Approx speed publisher for ACC self.speed_publisher = self.new_publisher( Float32, @@ -59,8 +63,14 @@ def __init__(self): self.__current_velocity: float = None self.__object_first_position: tuple = None self.__object_last_position: tuple = None + self.__last_position_oncoming: tuple = None + self.__first_position_oncoming: tuple = None self.logdebug("CollisionCheck started") + def __set_all_distances(self, data: Float32MultiArray): + self.__set_distance(data) + self.__set_distance_oncoming(data) + def update_distance(self, reset): """Updates the distance to the obstacle in front """ @@ -83,26 +93,54 @@ def __set_distance(self, data: Float32MultiArray): Args: data (Float32): Message from lidar with distance """ - nearest_object = filter_vision_objects(data.data) + nearest_object = filter_vision_objects(data.data, False) if nearest_object is None and \ self.__object_last_position is not None and \ rospy.get_rostime() - self.__object_last_position[0] > \ - rospy.Duration(2): + rospy.Duration(3): self.update_distance(True) return elif nearest_object is None: return - # self.logerr("Obstacle detected: " + str(nearest_object)) - # if np.isinf(data.data) and \ - # self.__object_last_position is not None and \ - # rospy.get_rostime() - self.__object_last_position[0] < \ - # rospy.Duration(1): - # return - # Set distance - 2 to clear distance from car self.__object_last_position = (rospy.get_rostime(), nearest_object[1]) self.update_distance(False) self.calculate_obstacle_speed() + def __set_distance_oncoming(self, data: Float32MultiArray): + """Saves last distance from LIDAR + + Args: + data (Float32): Message from lidar with distance + """ + nearest_object = filter_vision_objects(data.data, True) + if (nearest_object is None and + self.__last_position_oncoming is not None and + rospy.get_rostime() - self.__last_position_oncoming[0] > + rospy.Duration(3)): + self.update_distance_oncoming(True) + return + elif nearest_object is None: + return + + self.__last_position_oncoming = \ + (rospy.get_rostime(), nearest_object[1]) + self.update_distance_oncoming(False) + self.oncoming_pub.publish(Float32(data=nearest_object[1])) + + def update_distance_oncoming(self, reset): + """Updates the distance to the obstacle in front + """ + if reset: + # Reset all values if we do not have car in front + self.__last_position_oncoming = None + self.__first_position_oncoming = None + self.oncoming_pub.publish(Float32(data=np.inf)) + return + if self.__first_position_oncoming is None: + self.__first_position_oncoming = self.__last_position_oncoming + self.__last_position_oncoming = None + return + def calculate_obstacle_speed(self): """Caluclate the speed of the obstacle in front of the ego vehicle based on the distance between to timestamps @@ -183,10 +221,11 @@ def calculate_rule_of_thumb(emergency, speed): Returns: float: distance calculated with rule of thumb """ - reaction_distance = speed * 0.36 + reaction_distance = speed * 0.5 braking_distance = (speed * 0.36)**2 if emergency: - return reaction_distance + braking_distance / 2 + # Emergency brake is really effective in Carla + return reaction_distance + braking_distance / 4 else: return reaction_distance + braking_distance @@ -208,7 +247,7 @@ def check_crash(self, obstacle): if collision_time > 0: if distance < emergency_distance2: # Initiate emergency brake - self.logerr("Emergency Brake") + self.logerr(f"Emergency Brake: {distance}") self.emergency_pub.publish(True) # When no emergency brake is needed publish collision object data = Float32MultiArray(data=[distance, obstacle_speed]) diff --git a/code/planning/src/local_planner/motion_planning.py b/code/planning/src/local_planner/motion_planning.py index e55fd87b..10f57135 100755 --- a/code/planning/src/local_planner/motion_planning.py +++ b/code/planning/src/local_planner/motion_planning.py @@ -2,7 +2,6 @@ # import tf.transformations import ros_compatibility as roscomp import rospy -import tf.transformations from ros_compatibility.node import CompatibleNode from rospy import Publisher, Subscriber @@ -14,14 +13,11 @@ from scipy.spatial.transform import Rotation import math -from frenet_optimal_trajectory_planner.FrenetOptimalTrajectory.fot_wrapper \ - import run_fot from perception.msg import Waypoint, LaneChange import planning # noqa: F401 from behavior_agent.behaviours import behavior_speed as bs -from utils import convert_to_ms, approx_obstacle_pos, \ - hyperparameters, spawn_car, location_to_gps +from utils import convert_to_ms, spawn_car # from scipy.spatial._kdtree import KDTree @@ -45,6 +41,7 @@ def __init__(self): self.__stopline = None # (Distance, isStopline) self.__change_point = None # (Distance, isLaneChange, roadOption) self.__collision_point = None + self.__overtake_status = -1 self.published = False self.current_pos = None self.current_heading = None @@ -187,41 +184,6 @@ def __set_current_pos(self, data: PoseStamped): data.pose.position.y, data.pose.position.z]) - def calculate_overtake(self, distance, pose_list, limit_waypoints=100): - """Calculate new trajectory for overtaking - - Args: - distance (float): Distance to overtake object - pose_list (ndarray): List with current trajectory - limit_waypoints (int, optional): Number of waypoints. - Defaults to 30. - - Returns: - tuple: Return values from run_fot - """ - obstacle_position = approx_obstacle_pos(distance, - self.current_heading, - self.current_pos, - self.current_speed) - - selection = pose_list[int(self.current_wp):int(self.current_wp) + - int(distance + limit_waypoints)] - waypoints = self.convert_pose_to_array(selection) - gps_position = location_to_gps(0, 0, *self.current_pos[:2]) - initial_conditions = { - 'ps': gps_position["lon"], - 'target_speed': self.target_speed, - 'pos': np.array([self.current_pos[0], self.current_pos[1]]), - 'vel': np.array([obstacle_position[2][0], - obstacle_position[2][1]]), - 'wp': waypoints, - 'obs': np.array([[obstacle_position[0][0], - obstacle_position[0][1], - obstacle_position[1][0], - obstacle_position[1][1]]]) - } - return run_fot(initial_conditions, hyperparameters) - def change_trajectory(self, distance_obj): """update trajectory for overtaking and convert it to a new Path message @@ -230,57 +192,21 @@ def change_trajectory(self, distance_obj): distance_obj (float): distance to overtake object """ pose_list = self.trajectory.poses - count_retrys = 0 # Only use fallback self.overtake_fallback(distance_obj, pose_list) - self.overtake_success_pub.publish(1) + self.__overtake_status = 1 + self.overtake_success_pub.publish(self.__overtake_status) return - success_overtake = False - while not success_overtake and count_retrys < 10: - result_x, result_y, speeds, ix, iy, iyaw, d, s, speeds_x, \ - speeds_y, misc, \ - costs, success = self.calculate_overtake(distance_obj, - pose_list) - self.overtake_success_pub.publish(float(success)) - success_overtake = success - count_retrys += 1 - if success_overtake: - result = [] - for i in range(len(result_x)): - position = Point(result_x[i], result_y[i], 0) - quaternion = tf.transformations.quaternion_from_euler(0, - 0, - iyaw[i]) - orientation = Quaternion(x=quaternion[0], y=quaternion[1], - z=quaternion[2], w=quaternion[3]) - pose = Pose(position, orientation) - pos = PoseStamped() - pos.header.frame_id = "global" - pos.pose = pose - result.append(pos) - path = Path() - path.header.stamp = rospy.Time.now() - path.header.frame_id = "global" - path.poses = pose_list[:int(self.current_wp)] + \ - result + pose_list[int(self.current_wp + - distance_obj + - 30):] - # self.trajectory = path - else: - self.loginfo("Overtake failed") - self.overtake_fallback(distance_obj, pose_list) - self.overtake_success_pub.publish(1) - def overtake_fallback(self, distance, pose_list): - self.loginfo("Overtake Fallback!") + # self.loginfo("Overtake Fallback!") # obstacle_position = approx_obstacle_pos(distance, # self.current_heading, # self.current_pos, # self.current_speed) selection = pose_list[int(self.current_wp):int(self.current_wp) + - int(distance) + 10] + int(distance) + 7] waypoints = self.convert_pose_to_array(selection) offset = np.array([2, 0, 0]) @@ -290,8 +216,8 @@ def overtake_fallback(self, distance, pose_list): offset_front = offset_front[:2] waypoints_off = waypoints + offset_front - result_x = [row[0] for row in waypoints_off] - result_y = [row[1] for row in waypoints_off] + result_x = waypoints_off[:, 0] + result_y = waypoints_off[:, 1] result = [] for i in range(len(result_x)): @@ -312,7 +238,7 @@ def overtake_fallback(self, distance, pose_list): path.header.stamp = rospy.Time.now() path.header.frame_id = "global" path.poses = pose_list[:int(self.current_wp)] + \ - result + pose_list[int(self.current_wp + distance + 10):] + result + pose_list[int(self.current_wp + distance + 7):] self.trajectory = path def __set_trajectory(self, data: Path): @@ -441,11 +367,11 @@ def __check_emergency(self, data: Bool): def update_target_speed(self, acc_speed, behavior): be_speed = self.get_speed_by_behavior(behavior) - if not behavior == bs.parking.name: + if behavior == bs.parking.name or self.__overtake_status == 1: + self.target_speed = be_speed + else: corner_speed = self.get_cornering_speed() self.target_speed = min(be_speed, acc_speed, corner_speed) - else: - self.target_speed = be_speed # self.target_speed = min(self.target_speed, 8) self.velocity_pub.publish(self.target_speed) # self.logerr(f"Speed: {self.target_speed}") @@ -458,7 +384,8 @@ def __set_curr_behavior(self, data: String): self.__curr_behavior = data.data if data.data == bs.ot_enter_init.name: if np.isinf(self.__collision_point): - self.overtake_success_pub.publish(-1) + self.__overtake_status = -1 + self.overtake_success_pub.publish(self.__overtake_status) return self.change_trajectory(self.__collision_point) @@ -488,6 +415,7 @@ def get_speed_by_behavior(self, behavior: str) -> float: elif short_behavior == "parking": speed = bs.parking.speed else: + self.__overtake_status = -1 speed = self.__get_speed_cruise() return speed @@ -530,7 +458,7 @@ def __get_speed_overtake(self, behavior: str) -> float: if behavior == bs.ot_app_blocked.name: speed = self.__calc_speed_to_stop_overtake() elif behavior == bs.ot_app_free.name: - speed = self.__get_speed_cruise() + speed = self.__calc_speed_to_stop_overtake() elif behavior == bs.ot_wait_stopped.name: speed = bs.ot_wait_stopped.speed elif behavior == bs.ot_wait_free.name: @@ -540,7 +468,7 @@ def __get_speed_overtake(self, behavior: str) -> float: elif behavior == bs.ot_enter_slow.name: speed = self.__calc_speed_to_stop_overtake() elif behavior == bs.ot_leave.name: - speed = self.__get_speed_cruise() + speed = convert_to_ms(10.) return speed @@ -579,8 +507,10 @@ def __calc_speed_to_stop_overtake(self) -> float: v_stop = max(convert_to_ms(10.), convert_to_ms((stopline / 30) * 50)) - if stopline < 8.0: + if stopline < 6.0: v_stop = 0.0 + + self.logerr(f"Speed ovt: {v_stop}") return v_stop def __calc_virtual_change_point(self) -> float: diff --git a/code/planning/src/local_planner/utils.py b/code/planning/src/local_planner/utils.py index 35a5b7e8..c59675db 100644 --- a/code/planning/src/local_planner/utils.py +++ b/code/planning/src/local_planner/utils.py @@ -144,8 +144,12 @@ def spawn_car(distance): spawnPoint = carla.Transform(ego_vehicle.get_location() + carla.Location(y=distance.data), ego_vehicle.get_transform().rotation) + spawnpoint2 = carla.Transform(ego_vehicle.get_location() + + carla.Location(x=2.5, y=distance.data + 1), + ego_vehicle.get_transform().rotation) vehicle = world.spawn_actor(bp, spawnPoint) - + vehicle2 = world.spawn_actor(bp, spawnpoint2) + vehicle2.set_autopilot(False) vehicle.set_autopilot(False) # vehicle.set_location(loc) # coords = vehicle.get_location() @@ -157,7 +161,7 @@ def spawn_car(distance): carla.Rotation(pitch=-90))) -def filter_vision_objects(float_array): +def filter_vision_objects(float_array, oncoming): """Filters vision objects to calculate collision check It contains the classId, the absolute Euclidean distance and 6 coordinates for upper left and lower right corner @@ -193,7 +197,15 @@ def filter_vision_objects(float_array): all_cars = float_array[np.where(float_array[:, 0] == 2)] # Get cars that are on our lane - cars_in_front = all_cars[np.where(np.abs(all_cars[:, 2]) < 0.3)] + if oncoming: + cars_in_front = \ + all_cars[np.where(all_cars[:, 2] > 0.3)] + if cars_in_front.size != 0: + cars_in_front = cars_in_front[np.where(cars_in_front[:, 2] < 1.3)] + else: + cars_in_front = all_cars[np.where(all_cars[:, 2] < 0.1)] + if cars_in_front.size != 0: + cars_in_front = cars_in_front[np.where(cars_in_front[:, 2] > -0.2)] if cars_in_front.size == 0: # no car in front return None