From 98f4f9fec6b3c8089f0b67d52bc3180eca9f8f72 Mon Sep 17 00:00:00 2001 From: JuliusMiller Date: Thu, 25 Jan 2024 21:52:07 +0100 Subject: [PATCH] fix: intersection, lane_change, curve_detection --- code/agent/config/rviz_config.rviz | 2 +- .../behaviours/behavior_speed.py | 12 +- .../behavior_agent/behaviours/intersection.py | 18 +- .../behavior_agent/behaviours/lane_change.py | 12 +- .../src/local_planner/motion_planning.py | 166 +++++++++++++----- 5 files changed, 149 insertions(+), 61 deletions(-) diff --git a/code/agent/config/rviz_config.rviz b/code/agent/config/rviz_config.rviz index 42b620d5..2e419cc2 100644 --- a/code/agent/config/rviz_config.rviz +++ b/code/agent/config/rviz_config.rviz @@ -65,7 +65,7 @@ Visualization Manager: Visibility: Grid: false Imu: false - Path: true + Path: false PointCloud2: false Value: true Zoom Factor: 1 diff --git a/code/planning/src/behavior_agent/behaviours/behavior_speed.py b/code/planning/src/behavior_agent/behaviours/behavior_speed.py index 41e75530..66d2f2f3 100755 --- a/code/planning/src/behavior_agent/behaviours/behavior_speed.py +++ b/code/planning/src/behavior_agent/behaviours/behavior_speed.py @@ -20,7 +20,7 @@ def convert_to_ms(speed): int_app_init = Behavior("int_app_init", convert_to_ms(30.0)) # No Traffic Light or Sign -> stop dynamically at Stopline -int_app_no_sign = Behavior("int_app_no_sign", -2) +int_app_to_stop = Behavior("int_app_to_stop", -2) int_app_green = Behavior("int_app_green", convert_to_ms(30.0)) @@ -30,11 +30,7 @@ def convert_to_ms(speed): # Enter -int_enter_no_light = Behavior("int_enter_no_light", convert_to_ms(50.0)) - -int_enter_empty_str = Behavior("int_enter_empty_string", convert_to_ms(18.0)) - -int_enter_light = Behavior("int_enter_light", convert_to_ms(50.0)) +int_enter = Behavior("int_enter_no_light", convert_to_ms(50.0)) # Exit @@ -45,11 +41,11 @@ def convert_to_ms(speed): # Approach -lc_app_init = Behavior("lc_app_blocked", convert_to_ms(30.0)) +lc_app_init = Behavior("lc_app_init", convert_to_ms(30.0)) # TODO: Find out purpose of v_stop in lane_change (lines: 105 - 128) -lc_app_blocked = Behavior("lc_app_blocked", 0.5) +lc_app_blocked = Behavior("lc_app_blocked", -2) # Wait diff --git a/code/planning/src/behavior_agent/behaviours/intersection.py b/code/planning/src/behavior_agent/behaviours/intersection.py index bbb091c9..5f8eb151 100755 --- a/code/planning/src/behavior_agent/behaviours/intersection.py +++ b/code/planning/src/behavior_agent/behaviours/intersection.py @@ -104,7 +104,6 @@ def update(self): "/paf/hero/Center/traffic_light_state") if light_status_msg is not None: self.traffic_light_status = get_color(light_status_msg.state) - rospy.logerr(f"Light Status approach: {self.traffic_light_status}") self.traffic_light_detected = True # Update stopline Info @@ -138,7 +137,7 @@ def update(self): not self.traffic_light_detected): rospy.loginfo("slowing down!") - self.curr_behavior_pub.publish(bs.int_app_no_sign.name) + self.curr_behavior_pub.publish(bs.int_app_to_stop.name) # approach slowly when traffic light is green as traffic lights are # higher priority than traffic signs this behavior is desired @@ -230,6 +229,7 @@ def setup(self, timeout): "curr_behavior", String, queue_size=1) self.blackboard = py_trees.blackboard.Blackboard() + self.red_light_flag = False return True def initialise(self): @@ -243,6 +243,7 @@ def initialise(self): :return: True """ rospy.loginfo("Wait Intersection") + self.red_light_flag = False return True def update(self): @@ -279,9 +280,13 @@ def update(self): if light_status_msg is not None: traffic_light_status = get_color(light_status_msg.state) - rospy.logerr(f"Light Status wait: {traffic_light_status}") if traffic_light_status == "red" or \ traffic_light_status == "yellow": + self.red_light_flag = True + rospy.loginfo(f"Light Status: {traffic_light_status}") + self.curr_behavior_pub.publish(bs.int_wait.name) + return py_trees.common.Status.RUNNING + elif self.red_light_flag and traffic_light_status != "green": rospy.loginfo(f"Light Status: {traffic_light_status}") self.curr_behavior_pub.publish(bs.int_wait.name) return py_trees.common.Status.RUNNING @@ -358,14 +363,13 @@ def initialise(self): light_status_msg = self.blackboard.get( "/paf/hero/Center/traffic_light_state") if light_status_msg is None: - self.curr_behavior_pub.publish(bs.int_enter_no_light.name) + self.curr_behavior_pub.publish(bs.int_enter.name) return True traffic_light_status = get_color(light_status_msg.state) - rospy.logerr(f"Light Status ENTER: {traffic_light_status}") rospy.loginfo(f"Light Status: {traffic_light_status}") - self.curr_behavior_pub.publish(bs.int_enter_light.name) + self.curr_behavior_pub.publish(bs.int_enter.name) def update(self): """ @@ -391,7 +395,7 @@ def update(self): # not next_waypoint_msg.isStopLine: if next_waypoint_msg.distance < 5: rospy.loginfo("Drive through intersection!") - # self.update_local_path(leave_intersection=True) + self.curr_behavior_pub.publish(bs.int_enter.name) return py_trees.common.Status.RUNNING else: return py_trees.common.Status.SUCCESS diff --git a/code/planning/src/behavior_agent/behaviours/lane_change.py b/code/planning/src/behavior_agent/behaviours/lane_change.py index 4433cc2e..53b34d2b 100755 --- a/code/planning/src/behavior_agent/behaviours/lane_change.py +++ b/code/planning/src/behavior_agent/behaviours/lane_change.py @@ -70,7 +70,7 @@ def initialise(self): self.change_detected = False self.change_distance = np.inf self.virtual_change_distance = np.inf - self.curr_behavior_pub.publish(bs.lc_init.name) + self.curr_behavior_pub.publish(bs.lc_app_init.name) def update(self): """ @@ -111,13 +111,13 @@ def update(self): else: distance_lidar = None - if distance_lidar is not None and distance_lidar.min_range > 15.0: + distance_lidar = 20 # Remove and adjust to check for cars behind + + if distance_lidar is not None and distance_lidar > 15.0: rospy.loginfo("Change is free not slowing down!") - # self.update_local_path(leave_intersection=True) return py_trees.common.Status.SUCCESS else: rospy.loginfo("Change blocked slowing down") - self.curr_behavior_pub.publish(bs.lc_app_blocked.name) # get speed speedometer = self.blackboard.get("/carla/hero/Speed") @@ -242,10 +242,12 @@ def update(self): else: distance_lidar = None + distance_lidar = 20 # Remove to wait + change_clear = False if distance_lidar is not None: # if distance smaller than 15m, change is blocked - if distance_lidar.min_range < 15.0: + if distance_lidar < 15.0: change_clear = False else: change_clear = True diff --git a/code/planning/src/local_planner/motion_planning.py b/code/planning/src/local_planner/motion_planning.py index 9626df9b..eef50a01 100755 --- a/code/planning/src/local_planner/motion_planning.py +++ b/code/planning/src/local_planner/motion_planning.py @@ -13,9 +13,7 @@ import numpy as np -from nav_msgs.msg import Path -# from behavior_agent.msg import BehaviorSpeed from frenet_optimal_trajectory_planner.FrenetOptimalTrajectory.fot_wrapper \ import run_fot from perception.msg import Waypoint, LaneChange @@ -44,7 +42,7 @@ class MotionPlanning(CompatibleNode): def __init__(self): super(MotionPlanning, self).__init__('MotionPlanning') self.role_name = self.get_param("role_name", "hero") - self.control_loop_rate = self.get_param("control_loop_rate", 0.2) + self.control_loop_rate = self.get_param("control_loop_rate", 1) self.target_speed = 0.0 self.__curr_behavior = None @@ -53,13 +51,15 @@ def __init__(self): self.__change_point = None # (Distance, isLaneChange, roadOption) self.counter = 0 self.speed_list = [] - self.__trajectory = None + self.__first_trajectory = None + self.__corners = None + self.__in_corner = False # Subscriber self.trajectory_sub = self.new_subscription( Path, f"/paf/{self.role_name}/trajectory", - self.__set_trajectory, + self.__save_trajectory, qos_profile=1) self.current_pos_sub = self.new_subscription( PoseStamped, @@ -93,12 +93,6 @@ def __init__(self): f"/paf/{self.role_name}/lane_change_distance", self.__set_change_point, qos_profile=1) - - self.trajectory_sub: Subscriber = self.new_subscription( - Path, - f"/paf/{self.role_name}/trajectory", - self.__set_trajectory, - qos_profile=1) # Publisher self.traj_pub: Publisher = self.new_publisher( @@ -120,6 +114,12 @@ def __init__(self): self.published = False self.current_pos = None + def __save_trajectory(self, data): + if self.__first_trajectory is None: + self.__first_trajectory = data.poses + + self.__corners = self.__calc_corner_points() + def __set_current_pos(self, data: PoseStamped): """set current position @@ -141,7 +141,9 @@ def __set_trajectory(self, data: Path): selection = np_array[5:17] waypoints = self.convert_pose_to_array(selection) self.logerr(waypoints) - obs = np.array([[waypoints[5][0]-0.5, waypoints[5][1], waypoints[5][0]+0.5, waypoints[5][1]-2]]) + obs = np.array( + [[waypoints[5][0]-0.5, waypoints[5][1], waypoints[5][0]+0.5, + waypoints[5][1]-2]]) initial_conditions = { 'ps': 0, 'target_speed': self.__acc_speed, @@ -183,11 +185,10 @@ def __set_trajectory(self, data: Path): result = [] for i in range(len(result_x)): position = Point(result_x[i], result_y[i], 703) - quaternion = tf.transformations.quaternion_from_euler(0, - 0, - iyaw[i]) - orientation = Quaternion(x=quaternion[0], y=quaternion[1], - z=quaternion[2], w=quaternion[3]) + quat = tf.transformations.quaternion_from_euler(0, 0, + iyaw[i]) + orientation = Quaternion(x=quat[0], y=quat[1], + z=quat[2], w=quat[3]) pose = Pose(position, orientation) pos = PoseStamped() pos.header.stamp = rospy.Time.now() @@ -200,6 +201,92 @@ def __set_trajectory(self, data: Path): path.poses = list(np_array[:5]) + result + list(np_array[17:]) self.traj_pub.publish(path) + def __calc_corner_points(self): + coords = self.convert_pose_to_array(np.array(self.__first_trajectory)) + x_values = np.array([point[0] for point in coords]) + y_values = np.array([point[1] for point in coords]) + + angles = np.arctan2(np.diff(y_values), np.diff(x_values)) + angles = np.rad2deg(angles) + angles[angles > 0] -= 360 # Convert for angles between 0 - 360 degree + + threshold = 1 # in degree + curve_change_indices = np.where(np.abs(np.diff(angles)) > threshold)[0] + + sublist = self.create_sublists(curve_change_indices, proximity=5) + + coords_of_curve = [coords[i] for i in sublist] + + return coords_of_curve + + def create_sublists(self, points, proximity=5): + sublists = [] + current_sublist = [] + + for point in points: + if not current_sublist: + current_sublist.append(point) + else: + last_point = current_sublist[-1] + distance = abs(point - last_point) + + if distance <= proximity: + current_sublist.append(point) + else: + sublists.append(current_sublist) + current_sublist = [point] + if current_sublist: + sublists.append(current_sublist) + + filtered_list = [in_list for in_list in sublists if len(in_list) > 1] + + return filtered_list + + def get_cornering_speed(self): + corner = self.__corners[0] + pos = self.current_pos + + def euclid_dist(vector1, vector2): + point1 = np.array(vector1) + point2 = np.array(vector2) + + diff = point2 - point1 + sum_sqrt = np.dot(diff.T, diff) + return np.sqrt(sum_sqrt) + + def map_corner(dist): + if dist < 10: + return 5 + elif dist < 25: + return 6 + elif dist < 50: + return 7 + else: + 8 + + distance_corner = 0 + for i in range(len(corner) - 1): + distance_corner += euclid_dist(corner[i], corner[i + 1]) + # self.logerr(distance_corner) + + if self.__in_corner: + self.loginfo("In Corner") + + distance_end = euclid_dist(pos, corner[0]) + if distance_end > distance_corner + 2: + self.__in_corner = False + self.__corners.pop(0) + return self.__get_speed_cruise() + else: + return map_corner(distance_corner) + + distance_start = euclid_dist(pos, corner[0]) + if distance_start < 3: + self.__in_corner = True + return map_corner(distance_corner) + else: + return self.__get_speed_cruise() + def convert_pose_to_array(self, poses: np.array): """convert pose array to numpy array @@ -209,9 +296,11 @@ def convert_pose_to_array(self, poses: np.array): Returns: np.array: numpy array """ - def get_x_y(pose): - return np.array([pose.pose.position.x, pose.pose.position.y]) - return np.vectorize(get_x_y)(poses) + result_array = np.empty((len(poses), 2)) + for pose in range(len(poses)): + result_array[pose] = np.array([poses[pose].pose.position.x, + poses[pose].pose.position.y]) + return result_array def __check_emergency(self, data: Bool): """If an emergency stop is needed first check if we are @@ -232,6 +321,9 @@ def update_target_speed(self, acc_speed, behavior): else: self.target_speed = be_speed # self.logerr("target speed: " + str(self.target_speed)) + corner_speed = self.get_cornering_speed() + self.target_speed = min(self.target_speed, corner_speed) + # self.target_speed = min(self.target_speed, 8) self.velocity_pub.publish(self.target_speed) # self.logerr(f"Speed: {self.target_speed}") # self.speed_list.append(self.target_speed) @@ -239,9 +331,6 @@ def update_target_speed(self, acc_speed, behavior): def __set_acc_speed(self, data: Float32): self.__acc_speed = data.data - def __set_trajectory(self, data: Path): - self.__trajectory = data - def __set_curr_behavior(self, data: String): self.__curr_behavior = data.data self.update_target_speed(self.__acc_speed, self.__curr_behavior) @@ -258,9 +347,7 @@ def __set_change_point(self, data: LaneChange): def get_speed_by_behavior(self, behavior: str) -> float: speed = 0.0 split = "_" - self.loginfo("get speed") short_behavior = behavior.partition(split)[0] - self.loginfo("short behavior: " + str(short_behavior)) if short_behavior == "int": speed = self.__get_speed_intersection(behavior) elif short_behavior == "lc": @@ -277,16 +364,12 @@ def __get_speed_intersection(self, behavior: str) -> float: speed = bs.int_app_init.speed elif behavior == bs.int_app_green.name: speed = bs.int_app_green.speed - elif behavior == bs.int_app_no_sign.name: + elif behavior == bs.int_app_to_stop.name: speed = self.__calc_speed_to_stop_intersection() elif behavior == bs.int_wait.name: speed == bs.int_wait.speed - elif behavior == bs.int_enter_no_light: - speed = bs.int_enter_no_light.speed - elif behavior == bs.int_enter_empty_str.name: - speed = bs.int_enter_empty_str.speed - elif behavior == bs.int_enter_light.name: - speed == bs.int_enter_light.speed + elif behavior == bs.int_enter.name: + speed = bs.int_enter.speed elif behavior == bs.int_exit: speed = self.__get_speed_cruise() @@ -297,7 +380,7 @@ def __get_speed_lanechange(self, behavior: str) -> float: if behavior == bs.lc_app_init.name: speed = bs.lc_app_init.speed elif behavior == bs.lc_app_blocked.name: - speed = bs.lc_app_blocked.speed # calc_speed_to_stop_lanechange() + speed = self.__calc_speed_to_stop_lanechange() elif behavior == bs.lc_enter_init.name: speed = bs.lc_enter_init.speed elif behavior == bs.lc_exit.name: @@ -323,17 +406,20 @@ def __calc_speed_to_stop_intersection(self) -> float: # TODO: Find out purpose def __calc_speed_to_stop_lanechange(self) -> float: - if self.__change_point[0] != np.inf and self.__change_point[1]: - stopline = self.__change_point[0] - else: - return 100 + stopline = self.__calc_virtual_change_point() v_stop = max(convert_to_ms(5.), convert_to_ms((stopline / 30) ** 1.5 * 50)) - if v_stop > convert_to_ms(50.0): - v_stop = convert_to_ms(30.0) + if v_stop > bs.lc_app_init.speed: + v_stop = bs.lc_app_init.speed return v_stop + + def __calc_virtual_change_point(self) -> float: + if self.__change_point[0] != np.inf and self.__change_point[1]: + return self.__change_point[0] + else: + return 0.0 def __calc_virtual_stopline(self) -> float: if self.__stopline[0] != np.inf and self.__stopline[1]: @@ -348,7 +434,7 @@ def run(self): """ # def loop(timer_event=None): - # self.logerr(self.speed_list) + # self.__calc_corner_points() # self.new_timer(self.control_loop_rate, loop) self.spin()