From 96669327d09bdd2d26ea68917bd599f7704db507 Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Tue, 30 Jan 2024 14:58:57 +0100 Subject: [PATCH 01/33] fix: linter --- .flake8 | 4 +++- .../src/behavior_agent/behaviours/__init__.py | 2 +- .../03_planning/00_paf23/test_traj.py | 3 ++- .../object-detection-model_evaluation/pt.py | 23 +++++++++++++------ .../pylot.py | 9 +++++--- 5 files changed, 28 insertions(+), 13 deletions(-) diff --git a/.flake8 b/.flake8 index 5a3d3cd9..6cce5018 100644 --- a/.flake8 +++ b/.flake8 @@ -2,4 +2,6 @@ exclude= code/planning/src/behavior_agent/behavior_tree.py, code/planning/src/behavior_agent/behaviours/__init__.py, code/planning/src/behavior_agent/behaviours, - code/planning/__init__.py \ No newline at end of file + code/planning/__init__.py, + doc/02_development/templates/template_class_no_comments.py, + doc/02_development/templates/template_class.py \ No newline at end of file diff --git a/code/planning/src/behavior_agent/behaviours/__init__.py b/code/planning/src/behavior_agent/behaviours/__init__.py index 330a8f1d..eb73c823 100755 --- a/code/planning/src/behavior_agent/behaviours/__init__.py +++ b/code/planning/src/behavior_agent/behaviours/__init__.py @@ -1,3 +1,3 @@ -from . import intersection, lane_change, maneuvers, meta, road_features +from . import intersection, lane_change, overtake, maneuvers, meta, road_features from . import topics2blackboard, traffic_objects from . import behavior_speed diff --git a/doc/03_research/03_planning/00_paf23/test_traj.py b/doc/03_research/03_planning/00_paf23/test_traj.py index b4e1165a..97283e6c 100644 --- a/doc/03_research/03_planning/00_paf23/test_traj.py +++ b/doc/03_research/03_planning/00_paf23/test_traj.py @@ -1,4 +1,5 @@ -from frenet_optimal_trajectory_planner.FrenetOptimalTrajectory.fot_wrapper import run_fot +from frenet_optimal_trajectory_planner.FrenetOptimalTrajectory.fot_wrapper \ + import run_fot import numpy as np import matplotlib.pyplot as plt diff --git a/doc/06_perception/experiments/object-detection-model_evaluation/pt.py b/doc/06_perception/experiments/object-detection-model_evaluation/pt.py index 3b8ccc3f..145fbcfe 100644 --- a/doc/06_perception/experiments/object-detection-model_evaluation/pt.py +++ b/doc/06_perception/experiments/object-detection-model_evaluation/pt.py @@ -6,8 +6,11 @@ from time import perf_counter import torch import torchvision -from torchvision.models.detection.faster_rcnn import FasterRCNN_MobileNet_V3_Large_320_FPN_Weights, FasterRCNN_ResNet50_FPN_V2_Weights -from torchvision.models.detection.retinanet import RetinaNet_ResNet50_FPN_V2_Weights +from torchvision.models.detection.faster_rcnn import \ + FasterRCNN_MobileNet_V3_Large_320_FPN_Weights, \ + FasterRCNN_ResNet50_FPN_V2_Weights +from torchvision.models.detection.retinanet import \ + RetinaNet_ResNet50_FPN_V2_Weights from globals import IMAGE_BASE_FOLDER, IMAGES_FOR_TEST from torchvision.utils import draw_bounding_boxes from pathlib import Path @@ -17,7 +20,8 @@ from torchvision.transforms.functional import to_pil_image ALL_MODELS = { - 'fasterrcnn_mobilenet_v3_large_320_fpn': FasterRCNN_MobileNet_V3_Large_320_FPN_Weights, + 'fasterrcnn_mobilenet_v3_large_320_fpn': + FasterRCNN_MobileNet_V3_Large_320_FPN_Weights, 'fasterrcnn_resnet50_fpn_v2': FasterRCNN_ResNet50_FPN_V2_Weights, 'retinanet_resnet50_fpn_v2': RetinaNet_ResNet50_FPN_V2_Weights, } @@ -28,7 +32,9 @@ def load_model(model_name): print('Loading model...', end='') device = torch.device("cuda" if torch.cuda.is_available() else "cpu") weights = ALL_MODELS[model_name].DEFAULT - model = torchvision.models.detection.__dict__[model_name](weights=weights).to(device) + model = torchvision.models.detection.__dict__[model_name]( + weights=weights + ).to(device) model.eval() return model, weights, device @@ -73,18 +79,21 @@ def load_image(image_path, model_weights, device): label_id_offset = -1 - image_np_with_detections = torch.tensor(image_np * 255, dtype=torch.uint8) + image_np_with_detections = torch.tensor(image_np * 255, + dtype=torch.uint8) boxes = result['boxes'] scores = result['scores'] labels = [weights.meta["categories"][i] for i in result['labels']] - box = draw_bounding_boxes(image_np_with_detections[0], boxes, labels, colors='red', width=2) + box = draw_bounding_boxes(image_np_with_detections[0], boxes, labels, + colors='red', width=2) box_img = to_pil_image(box) file_name = Path(image_path).stem plt.figure(figsize=(32, 18)) - plt.title(f'PyTorch - {m} - {p} - {elapsed_time*1000:.0f}ms', fontsize=30) + plt.title(f'PyTorch - {m} - {p} - {elapsed_time*1000:.0f}ms', + fontsize=30) plt.imshow(box_img) plt.savefig(f'{IMAGE_BASE_FOLDER}/result/{file_name}_PT_{m}.jpg') plt.close() diff --git a/doc/06_perception/experiments/object-detection-model_evaluation/pylot.py b/doc/06_perception/experiments/object-detection-model_evaluation/pylot.py index f3a670c6..d59e5e75 100644 --- a/doc/06_perception/experiments/object-detection-model_evaluation/pylot.py +++ b/doc/06_perception/experiments/object-detection-model_evaluation/pylot.py @@ -1,5 +1,6 @@ ''' -Docs: https://www.tensorflow.org/hub/tutorials/tf2_object_detection, https://pylot.readthedocs.io/en/latest/perception.detection.html +Docs: https://www.tensorflow.org/hub/tutorials/tf2_object_detection, +https://pylot.readthedocs.io/en/latest/perception.detection.html ''' from globals import IMAGE_BASE_FOLDER, IMAGES_FOR_TEST @@ -61,7 +62,8 @@ def get_category_index(label_file): with open(label_file, 'r') as f: labels = f.readlines() labels = [label.strip() for label in labels] - category_index = {i: {'id': i, 'name': name} for i, name in enumerate(labels)} + category_index = \ + {i: {'id': i, 'name': name} for i, name in enumerate(labels)} return category_index @@ -114,7 +116,8 @@ def get_category_index(label_file): file_name = Path(image_path).stem plt.figure(figsize=(32, 18)) - plt.title(f'Pylot (TF) - {m} - {p} - {elapsed_time*1000:.0f}ms', fontsize=30) + plt.title(f'Pylot (TF) - {m} - {p} - {elapsed_time*1000:.0f}ms', + fontsize=30) plt.imshow(image_np_with_detections[0]) plt.savefig(f'{IMAGE_BASE_FOLDER}/result/{file_name}_TF_{m}.jpg') plt.close() From 3f5bb60e144b7d012d803af148abfc100a58a7c8 Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Tue, 30 Jan 2024 16:54:23 +0100 Subject: [PATCH 02/33] feat: behaviors complete for overtake --- build/docker-compose.yml | 4 +- code/agent/launch/dev.launch | 2 +- .../src/global_plan_distance_publisher.py | 2 +- code/planning/launch/planning.launch | 4 +- .../behaviours/behavior_speed.py | 15 +- .../src/behavior_agent/behaviours/overtake.py | 27 ++- .../behaviours/topics2blackboard.py | 4 +- .../src/global_planner/global_planner.py | 6 +- code/planning/src/local_planner/ACC.py | 3 + .../src/local_planner/motion_planning.py | 169 ++++++++++++------ code/test-route/launch/test-route.launch | 2 +- 11 files changed, 167 insertions(+), 71 deletions(-) diff --git a/build/docker-compose.yml b/build/docker-compose.yml index 1372dfdb..6ba17e3d 100644 --- a/build/docker-compose.yml +++ b/build/docker-compose.yml @@ -62,8 +62,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=paf23-carla-simulator-1 --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=paf23-carla-simulator-1 --track=MAP" logging: driver: "local" diff --git a/code/agent/launch/dev.launch b/code/agent/launch/dev.launch index 071e9813..cc1f148d 100644 --- a/code/agent/launch/dev.launch +++ b/code/agent/launch/dev.launch @@ -32,7 +32,7 @@ - + diff --git a/code/perception/src/global_plan_distance_publisher.py b/code/perception/src/global_plan_distance_publisher.py index 40da367c..4e4cf37b 100755 --- a/code/perception/src/global_plan_distance_publisher.py +++ b/code/perception/src/global_plan_distance_publisher.py @@ -47,7 +47,7 @@ def __init__(self): # Change comment for dev_launch self.global_plan_subscriber = self.new_subscription( CarlaRoute, - "/carla/" + self.role_name + "/global_plan", + "/paf/" + self.role_name + "/global_plan", self.update_global_route, qos_profile=1) # self.global_plan_subscriber = self.new_subscription( diff --git a/code/planning/launch/planning.launch b/code/planning/launch/planning.launch index 9dee1352..72c1f88c 100644 --- a/code/planning/launch/planning.launch +++ b/code/planning/launch/planning.launch @@ -16,13 +16,13 @@ - + diff --git a/code/planning/src/behavior_agent/behaviours/behavior_speed.py b/code/planning/src/behavior_agent/behaviours/behavior_speed.py index df88e762..248edd49 100755 --- a/code/planning/src/behavior_agent/behaviours/behavior_speed.py +++ b/code/planning/src/behavior_agent/behaviours/behavior_speed.py @@ -47,7 +47,7 @@ def convert_to_ms(speed): # TODO: Find out purpose of v_stop in lane_change (lines: 105 - 128) lc_app_blocked = Behavior("lc_app_blocked", -2) -lc_app_free = Behavior("lc_app_free", convert_to_ms(20)) +lc_app_free = Behavior("lc_app_free", convert_to_ms(30.0)) # Wait lc_wait = Behavior("lc_wait", 0) @@ -63,6 +63,19 @@ def convert_to_ms(speed): lc_exit = Behavior("lc_exit", -1) # Get SpeedLimit dynamically +# Overtake + +# Approach +ot_app_blocked = Behavior("ot_app_blocked", -2) +ot_app_free = Behavior("ot_app_free", -1) +# Wait +ot_wait_stopped = Behavior("ot_wait_stopped", convert_to_ms(0.0)) +ot_wait_free = Behavior("ot_wait_free", -1) +# Enter +ot_enter_init = Behavior("ot_enter_init", -1) +ot_enter_slow = Behavior("ot_enter_slow", -2) +# Exit +ot_leave = Behavior("ot_leave", -1) # Cruise cruise = Behavior("Cruise", -1) diff --git a/code/planning/src/behavior_agent/behaviours/overtake.py b/code/planning/src/behavior_agent/behaviours/overtake.py index 36bc2c36..8c8bf7d5 100644 --- a/code/planning/src/behavior_agent/behaviours/overtake.py +++ b/code/planning/src/behavior_agent/behaviours/overtake.py @@ -2,6 +2,7 @@ from std_msgs.msg import String import rospy +import numpy as np from . import behavior_speed as bs @@ -86,9 +87,9 @@ def update(self): py_trees.common.Status.FAILURE, """ # Update distance to collision object - _dis = self.blackboard.get("/paf/hero/LIDAR_range") + _dis = self.blackboard.get("/paf/hero/collision") if _dis is not None: - self.ot_distance = _dis.data + self.ot_distance = _dis.data[0] rospy.loginfo(f"Overtake distance: {self.ot_distance}") # slow down before overtake if blocked @@ -103,6 +104,7 @@ def update(self): if distance_lidar is not None and \ distance_lidar > 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 else: rospy.loginfo("Overtake blocked slowing down") @@ -229,12 +231,13 @@ def update(self): collision_distance = distance_lidar.data if collision_distance > 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") self.curr_behavior_pub.publish(bs.ot_wait_stopped.name) return py_trees.commom.Status.RUNNING - elif obstacle_msg[1] > convert_to_ms(2): + elif obstacle_msg.data[0] == np.inf: return py_trees.common.Status.FAILURE else: rospy.loginfo("No Lidar Distance") @@ -315,9 +318,21 @@ def update(self): py_trees.common.Status.SUCCESS, py_trees.common.Status.FAILURE, """ - - return py_trees.common.Status.SUCCESS - + status = self.blackboard.get("/paf/hero/overtake_success") + if status is not None: + if status.data == 1: + rospy.loginfo("Overtake: Trajectory planned") + return py_trees.common.Status.SUCCESS + elif status.data == 0: + self.curr_behavior_pub.publish(bs.ot_enter_slow.name) + rospy.loginfo("Overtake: Slowing down") + return py_trees.common.Status.RUNNING + else: + rospy.loginfo("Overtake: Big Failure") + return py_trees.common.Status.FAILURE + else: + rospy.loginfo("Overtake: Bigger Failure") + return py_trees.common.Status.FAILURE # Currently not in use # Can be used to check if we can go back to the original lane diff --git a/code/planning/src/behavior_agent/behaviours/topics2blackboard.py b/code/planning/src/behavior_agent/behaviours/topics2blackboard.py index 86f3c3b1..04e22f59 100755 --- a/code/planning/src/behavior_agent/behaviours/topics2blackboard.py +++ b/code/planning/src/behavior_agent/behaviours/topics2blackboard.py @@ -47,7 +47,9 @@ def create_node(role_name): {'name': f"/paf/{role_name}/collision", 'msg': Float32MultiArray, 'clearing-policy': py_trees.common.ClearingPolicy.ON_INITIALISE}, {'name': f"/paf/{role_name}/current_pos", 'msg': PoseStamped, - 'clearing-policy': py_trees.common.ClearingPolicy.NEVER} + 'clearing-policy': py_trees.common.ClearingPolicy.NEVER}, + {'name': f"/paf/{role_name}/overtake_success", 'msg': Float32, + 'clearing-policy': py_trees.common.ClearingPolicy.NEVER}, ] topics2blackboard = py_trees.composites.Parallel("Topics to Blackboard") diff --git a/code/planning/src/global_planner/global_planner.py b/code/planning/src/global_planner/global_planner.py index 41ee798c..26f806a6 100755 --- a/code/planning/src/global_planner/global_planner.py +++ b/code/planning/src/global_planner/global_planner.py @@ -57,7 +57,7 @@ def __init__(self): # uncomment /paf/hero/global_plan and comment /carla/... for dev_launch self.global_plan_sub = self.new_subscription( msg_type=CarlaRoute, - topic='/carla/' + self.role_name + '/global_plan', + topic='/paf/' + self.role_name + '/global_plan', callback=self.global_route_callback, qos_profile=10) # self.global_plan_sub = self.new_subscription( @@ -84,7 +84,7 @@ def __init__(self): self.logdebug('PrePlanner-Node started') # uncomment for self.dev_load_world_info() for dev_launch - # self.dev_load_world_info() + self.dev_load_world_info() def global_route_callback(self, data: CarlaRoute) -> None: """ @@ -194,7 +194,7 @@ def global_route_callback(self, data: CarlaRoute) -> None: self.path_backup.header.frame_id = "global" self.path_backup.poses = stamped_poses self.path_pub.publish(self.path_backup) - self.loginfo("PrePlanner: published trajectory") + self.logerr("PrePlanner: published trajectory") def world_info_callback(self, opendrive: String) -> None: """ diff --git a/code/planning/src/local_planner/ACC.py b/code/planning/src/local_planner/ACC.py index e09bc575..1c6a388a 100755 --- a/code/planning/src/local_planner/ACC.py +++ b/code/planning/src/local_planner/ACC.py @@ -198,6 +198,9 @@ def loop(timer_event=None): # If we have no obstacle, we want to drive with the current # speed limit self.velocity_pub.publish(self.speed_limit) + else: + self.logerr("ACC: No speed limit recieved") + self.velocity_pub.publish(5.0) self.new_timer(self.control_loop_rate, loop) self.spin() diff --git a/code/planning/src/local_planner/motion_planning.py b/code/planning/src/local_planner/motion_planning.py index e72396b5..97f5bf83 100755 --- a/code/planning/src/local_planner/motion_planning.py +++ b/code/planning/src/local_planner/motion_planning.py @@ -6,7 +6,7 @@ from ros_compatibility.node import CompatibleNode from rospy import Publisher, Subscriber -from std_msgs.msg import String, Float32, Bool +from std_msgs.msg import String, Float32, Bool, Float32MultiArray from nav_msgs.msg import Path from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion from carla_msgs.msg import CarlaSpeedometer @@ -42,6 +42,7 @@ def __init__(self): self.__acc_speed = 0.0 self.__stopline = None # (Distance, isStopline) self.__change_point = None # (Distance, isLaneChange, roadOption) + self.__collision_point = None self.published = False self.current_pos = None self.current_heading = None @@ -53,12 +54,14 @@ def __init__(self): self.current_speed = None self.speed_limit = None - self.counter = 0 - self.speed_list = [] - self.__first_trajectory = None self.__corners = None self.__in_corner = False # Subscriber + self.test_sub = self.new_subscription( + Float32, + f"/paf/{self.role_name}/test", + self.change_trajectory, + qos_profile=1) self.speed_limit_sub = self.new_subscription( Float32, f"/paf/{self.role_name}/speed_limit", @@ -113,6 +116,12 @@ def __init__(self): self.__set_change_point, qos_profile=1) + self.change_point_sub: Subscriber = self.new_subscription( + Float32MultiArray, + f"/paf/{self.role_name}/collision", + self.__set_collision_point, + qos_profile=1) + # Publisher self.traj_pub: Publisher = self.new_publisher( msg_type=Path, @@ -128,10 +137,9 @@ def __init__(self): f"/paf/{self.role_name}/current_wp", self.__set_wp, qos_profile=1) - # Publisher for emergency stop - self.emergency_pub = self.new_publisher( - Bool, - f"/paf/{self.role_name}/emergency", + self.overtake_success_pub = self.new_publisher( + Float32, + f"/paf/{self.role_name}/overtake_success", qos_profile=1) self.logdebug("MotionPlanning started") @@ -169,12 +177,6 @@ def __set_heading(self, data: Float32): """ self.current_heading = data.data - 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 Args: @@ -184,36 +186,47 @@ def __set_current_pos(self, data: PoseStamped): data.pose.position.y, data.pose.position.z]) - def change_trajectory(self, distance: float): + def change_trajectory(self, distance_obj: float): + distance_obj = distance_obj.data self.overtake_start = rospy.get_rostime() limit_waypoints = 30 - np_array = np.array(self.trajectory.poses) - obstacle_position = approx_obstacle_pos(distance, - self.current_heading, - self.current_pos, - self.current_speed) - # trajectory_np = self.convert_pose_to_array(np_array) - # wp = KDTree(trajectory_np[:, :2]).query(obstacle_position[0][:2])[1] - selection = np_array[int(self.current_wp):int(self.current_wp) + - int(distance + limit_waypoints)] - waypoints = self.convert_pose_to_array(selection) - - initial_conditions = { - 'ps': 0, - 'target_speed': self.current_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]]]) - } - result_x, result_y, speeds, ix, iy, iyaw, d, s, speeds_x, \ - speeds_y, misc, costs, success = run_fot(initial_conditions, - hyperparameters) - if success: + pose_list = self.trajectory.poses + count_retrys = 0 + + def calculate_overtake(distance): + obstacle_position = approx_obstacle_pos(distance, + self.current_heading, + self.current_pos, + self.current_speed) + # trajectory_np = self.convert_pose_to_array(pose_list) + # wp=KDTree(trajectory_np[:,:2]).query(obstacle_position[0][:2])[1] + selection = pose_list[int(self.current_wp):int(self.current_wp) + + int(distance + limit_waypoints)] + waypoints = self.convert_pose_to_array(selection) + + initial_conditions = { + 'ps': 0, + 'target_speed': self.current_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) + + 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 = calculate_overtake(distance_obj) + 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) @@ -230,9 +243,14 @@ def change_trajectory(self, distance: float): path = Path() path.header.stamp = rospy.Time.now() path.header.frame_id = "global" - path.poses = list(np_array[:int(self.current_wp)]) + \ - result + list(np_array[int(self.current_wp + 25 + 30):]) + path.poses = pose_list[:int(self.current_wp)] + \ + result + pose_list[int(self.current_wp + + distance_obj + + 30):] self.trajectory = path + else: + self.logerr("Overtake failed") + self.overtake_success_pub.publish(-1) def __set_trajectory(self, data: Path): """get current trajectory global planning @@ -241,9 +259,11 @@ def __set_trajectory(self, data: Path): data (Path): Trajectory waypoints """ self.trajectory = data + self.logerr("Trajectory received") + self.__corners = self.__calc_corner_points() def __calc_corner_points(self): - coords = self.convert_pose_to_array(np.array(self.__first_trajectory)) + coords = self.convert_pose_to_array(np.array(self.trajectory.poses)) x_values = np.array([point[0] for point in coords]) y_values = np.array([point[1] for point in coords]) @@ -285,7 +305,7 @@ def create_sublists(self, points, proximity=5): def get_cornering_speed(self): corner = self.__corners[0] - pos = self.current_pos + pos = self.current_pos[:2] def euclid_dist(vector1, vector2): point1 = np.array(vector1) @@ -358,12 +378,10 @@ 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: - self.target_speed = min(be_speed, acc_speed) + corner_speed = self.get_cornering_speed() + self.target_speed = min(be_speed, acc_speed, corner_speed, 6) 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}") @@ -384,6 +402,10 @@ def __set_change_point(self, data: LaneChange): self.__change_point = \ (data.distance, data.isLaneChange, data.roadOption) + def __set_collision_point(self, data: Float32MultiArray): + if data is not None: + self.__collision_point = data.data[0] + def get_speed_by_behavior(self, behavior: str) -> float: speed = 0.0 split = "_" @@ -392,6 +414,8 @@ def get_speed_by_behavior(self, behavior: str) -> float: speed = self.__get_speed_intersection(behavior) elif short_behavior == "lc": speed = self.__get_speed_lanechange(behavior) + elif short_behavior == "ot": + speed = self.__get_speed_overtake(behavior) elif short_behavior == "parking": speed = bs.parking.speed else: @@ -432,6 +456,25 @@ def __get_speed_lanechange(self, behavior: str) -> float: return speed + def __get_speed_overtake(self, behavior: str) -> float: + speed = 0.0 + 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() + elif behavior == bs.ot_wait_stopped.name: + speed = bs.ot_wait_stopped.speed + elif behavior == bs.ot_wait_free.name: + speed == self.__get_speed_cruise() + elif behavior == bs.ot_enter_init.name: + speed = self.__get_speed_cruise() + 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() + + return speed + def __get_speed_cruise(self) -> float: return self.__acc_speed @@ -461,6 +504,17 @@ def __calc_speed_to_stop_lanechange(self) -> float: v_stop = 0.0 return v_stop + def __calc_speed_to_stop_overtake(self) -> float: + stopline = self.__calc_virtual_overtake() + self.logerr(stopline) + + v_stop = max(convert_to_ms(10.), + convert_to_ms((stopline / 30) + * 50)) + if stopline < 3.0: + v_stop = 0.0 + 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] @@ -473,6 +527,14 @@ def __calc_virtual_stopline(self) -> float: else: return 0.0 + def __calc_virtual_overtake(self) -> float: + self.logerr(f"Test: {self.__collision_point}") + if (self.__collision_point is not None) and \ + self.__collision_point != np.inf: + return self.__collision_point + else: + return 0.0 + def run(self): """ Control loop @@ -483,11 +545,12 @@ def loop(timer_event=None): if (self.__curr_behavior is not None and self.__acc_speed is not None and self.__corners is not None): - self.update_target_speed(self.__acc_speed, - self.__curr_behavior) self.trajectory.header.stamp = rospy.Time.now() self.traj_pub.publish(self.trajectory) - + self.update_target_speed(self.__acc_speed, + self.__curr_behavior) + else: + self.velocity_pub.publish(0.0) self.new_timer(self.control_loop_rate, loop) self.spin() diff --git a/code/test-route/launch/test-route.launch b/code/test-route/launch/test-route.launch index de985c6b..189d9ace 100644 --- a/code/test-route/launch/test-route.launch +++ b/code/test-route/launch/test-route.launch @@ -6,7 +6,7 @@ - + From 92770080039c783ef8146a5092c9389ec40a0ba1 Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Tue, 30 Jan 2024 16:56:26 +0100 Subject: [PATCH 03/33] feat: change --- code/planning/src/local_planner/motion_planning.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/code/planning/src/local_planner/motion_planning.py b/code/planning/src/local_planner/motion_planning.py index 97f5bf83..2054542c 100755 --- a/code/planning/src/local_planner/motion_planning.py +++ b/code/planning/src/local_planner/motion_planning.py @@ -206,7 +206,7 @@ def calculate_overtake(distance): initial_conditions = { 'ps': 0, - 'target_speed': self.current_speed, + '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]]), From ff9dda694d949e2d00a0e689263dbbde67bc8565 Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Tue, 30 Jan 2024 14:58:57 +0100 Subject: [PATCH 04/33] fix: linter --- .flake8 | 4 +++- .../src/behavior_agent/behaviours/__init__.py | 2 +- .../03_planning/00_paf23/test_traj.py | 3 ++- .../object-detection-model_evaluation/pt.py | 23 +++++++++++++------ .../pylot.py | 9 +++++--- 5 files changed, 28 insertions(+), 13 deletions(-) diff --git a/.flake8 b/.flake8 index 5a3d3cd9..6cce5018 100644 --- a/.flake8 +++ b/.flake8 @@ -2,4 +2,6 @@ exclude= code/planning/src/behavior_agent/behavior_tree.py, code/planning/src/behavior_agent/behaviours/__init__.py, code/planning/src/behavior_agent/behaviours, - code/planning/__init__.py \ No newline at end of file + code/planning/__init__.py, + doc/02_development/templates/template_class_no_comments.py, + doc/02_development/templates/template_class.py \ No newline at end of file diff --git a/code/planning/src/behavior_agent/behaviours/__init__.py b/code/planning/src/behavior_agent/behaviours/__init__.py index 330a8f1d..eb73c823 100755 --- a/code/planning/src/behavior_agent/behaviours/__init__.py +++ b/code/planning/src/behavior_agent/behaviours/__init__.py @@ -1,3 +1,3 @@ -from . import intersection, lane_change, maneuvers, meta, road_features +from . import intersection, lane_change, overtake, maneuvers, meta, road_features from . import topics2blackboard, traffic_objects from . import behavior_speed diff --git a/doc/03_research/03_planning/00_paf23/test_traj.py b/doc/03_research/03_planning/00_paf23/test_traj.py index b4e1165a..97283e6c 100644 --- a/doc/03_research/03_planning/00_paf23/test_traj.py +++ b/doc/03_research/03_planning/00_paf23/test_traj.py @@ -1,4 +1,5 @@ -from frenet_optimal_trajectory_planner.FrenetOptimalTrajectory.fot_wrapper import run_fot +from frenet_optimal_trajectory_planner.FrenetOptimalTrajectory.fot_wrapper \ + import run_fot import numpy as np import matplotlib.pyplot as plt diff --git a/doc/06_perception/experiments/object-detection-model_evaluation/pt.py b/doc/06_perception/experiments/object-detection-model_evaluation/pt.py index 3b8ccc3f..145fbcfe 100644 --- a/doc/06_perception/experiments/object-detection-model_evaluation/pt.py +++ b/doc/06_perception/experiments/object-detection-model_evaluation/pt.py @@ -6,8 +6,11 @@ from time import perf_counter import torch import torchvision -from torchvision.models.detection.faster_rcnn import FasterRCNN_MobileNet_V3_Large_320_FPN_Weights, FasterRCNN_ResNet50_FPN_V2_Weights -from torchvision.models.detection.retinanet import RetinaNet_ResNet50_FPN_V2_Weights +from torchvision.models.detection.faster_rcnn import \ + FasterRCNN_MobileNet_V3_Large_320_FPN_Weights, \ + FasterRCNN_ResNet50_FPN_V2_Weights +from torchvision.models.detection.retinanet import \ + RetinaNet_ResNet50_FPN_V2_Weights from globals import IMAGE_BASE_FOLDER, IMAGES_FOR_TEST from torchvision.utils import draw_bounding_boxes from pathlib import Path @@ -17,7 +20,8 @@ from torchvision.transforms.functional import to_pil_image ALL_MODELS = { - 'fasterrcnn_mobilenet_v3_large_320_fpn': FasterRCNN_MobileNet_V3_Large_320_FPN_Weights, + 'fasterrcnn_mobilenet_v3_large_320_fpn': + FasterRCNN_MobileNet_V3_Large_320_FPN_Weights, 'fasterrcnn_resnet50_fpn_v2': FasterRCNN_ResNet50_FPN_V2_Weights, 'retinanet_resnet50_fpn_v2': RetinaNet_ResNet50_FPN_V2_Weights, } @@ -28,7 +32,9 @@ def load_model(model_name): print('Loading model...', end='') device = torch.device("cuda" if torch.cuda.is_available() else "cpu") weights = ALL_MODELS[model_name].DEFAULT - model = torchvision.models.detection.__dict__[model_name](weights=weights).to(device) + model = torchvision.models.detection.__dict__[model_name]( + weights=weights + ).to(device) model.eval() return model, weights, device @@ -73,18 +79,21 @@ def load_image(image_path, model_weights, device): label_id_offset = -1 - image_np_with_detections = torch.tensor(image_np * 255, dtype=torch.uint8) + image_np_with_detections = torch.tensor(image_np * 255, + dtype=torch.uint8) boxes = result['boxes'] scores = result['scores'] labels = [weights.meta["categories"][i] for i in result['labels']] - box = draw_bounding_boxes(image_np_with_detections[0], boxes, labels, colors='red', width=2) + box = draw_bounding_boxes(image_np_with_detections[0], boxes, labels, + colors='red', width=2) box_img = to_pil_image(box) file_name = Path(image_path).stem plt.figure(figsize=(32, 18)) - plt.title(f'PyTorch - {m} - {p} - {elapsed_time*1000:.0f}ms', fontsize=30) + plt.title(f'PyTorch - {m} - {p} - {elapsed_time*1000:.0f}ms', + fontsize=30) plt.imshow(box_img) plt.savefig(f'{IMAGE_BASE_FOLDER}/result/{file_name}_PT_{m}.jpg') plt.close() diff --git a/doc/06_perception/experiments/object-detection-model_evaluation/pylot.py b/doc/06_perception/experiments/object-detection-model_evaluation/pylot.py index f3a670c6..d59e5e75 100644 --- a/doc/06_perception/experiments/object-detection-model_evaluation/pylot.py +++ b/doc/06_perception/experiments/object-detection-model_evaluation/pylot.py @@ -1,5 +1,6 @@ ''' -Docs: https://www.tensorflow.org/hub/tutorials/tf2_object_detection, https://pylot.readthedocs.io/en/latest/perception.detection.html +Docs: https://www.tensorflow.org/hub/tutorials/tf2_object_detection, +https://pylot.readthedocs.io/en/latest/perception.detection.html ''' from globals import IMAGE_BASE_FOLDER, IMAGES_FOR_TEST @@ -61,7 +62,8 @@ def get_category_index(label_file): with open(label_file, 'r') as f: labels = f.readlines() labels = [label.strip() for label in labels] - category_index = {i: {'id': i, 'name': name} for i, name in enumerate(labels)} + category_index = \ + {i: {'id': i, 'name': name} for i, name in enumerate(labels)} return category_index @@ -114,7 +116,8 @@ def get_category_index(label_file): file_name = Path(image_path).stem plt.figure(figsize=(32, 18)) - plt.title(f'Pylot (TF) - {m} - {p} - {elapsed_time*1000:.0f}ms', fontsize=30) + plt.title(f'Pylot (TF) - {m} - {p} - {elapsed_time*1000:.0f}ms', + fontsize=30) plt.imshow(image_np_with_detections[0]) plt.savefig(f'{IMAGE_BASE_FOLDER}/result/{file_name}_TF_{m}.jpg') plt.close() From 1bc3b06a896e6d7de52649832d6732b1a65b7a1d Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Tue, 30 Jan 2024 16:54:23 +0100 Subject: [PATCH 05/33] feat: behaviors complete for overtake --- build/docker-compose.yml | 4 +- code/agent/launch/dev.launch | 2 +- .../src/global_plan_distance_publisher.py | 2 +- code/planning/launch/planning.launch | 4 +- .../behaviours/behavior_speed.py | 15 +- .../src/behavior_agent/behaviours/overtake.py | 27 ++- .../behaviours/topics2blackboard.py | 4 +- .../src/global_planner/global_planner.py | 6 +- code/planning/src/local_planner/ACC.py | 3 + .../src/local_planner/motion_planning.py | 169 ++++++++++++------ code/test-route/launch/test-route.launch | 2 +- 11 files changed, 167 insertions(+), 71 deletions(-) diff --git a/build/docker-compose.yml b/build/docker-compose.yml index 1372dfdb..6ba17e3d 100644 --- a/build/docker-compose.yml +++ b/build/docker-compose.yml @@ -62,8 +62,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=paf23-carla-simulator-1 --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=paf23-carla-simulator-1 --track=MAP" logging: driver: "local" diff --git a/code/agent/launch/dev.launch b/code/agent/launch/dev.launch index 071e9813..cc1f148d 100644 --- a/code/agent/launch/dev.launch +++ b/code/agent/launch/dev.launch @@ -32,7 +32,7 @@ - + diff --git a/code/perception/src/global_plan_distance_publisher.py b/code/perception/src/global_plan_distance_publisher.py index 40da367c..4e4cf37b 100755 --- a/code/perception/src/global_plan_distance_publisher.py +++ b/code/perception/src/global_plan_distance_publisher.py @@ -47,7 +47,7 @@ def __init__(self): # Change comment for dev_launch self.global_plan_subscriber = self.new_subscription( CarlaRoute, - "/carla/" + self.role_name + "/global_plan", + "/paf/" + self.role_name + "/global_plan", self.update_global_route, qos_profile=1) # self.global_plan_subscriber = self.new_subscription( diff --git a/code/planning/launch/planning.launch b/code/planning/launch/planning.launch index 9dee1352..72c1f88c 100644 --- a/code/planning/launch/planning.launch +++ b/code/planning/launch/planning.launch @@ -16,13 +16,13 @@ - + diff --git a/code/planning/src/behavior_agent/behaviours/behavior_speed.py b/code/planning/src/behavior_agent/behaviours/behavior_speed.py index df88e762..248edd49 100755 --- a/code/planning/src/behavior_agent/behaviours/behavior_speed.py +++ b/code/planning/src/behavior_agent/behaviours/behavior_speed.py @@ -47,7 +47,7 @@ def convert_to_ms(speed): # TODO: Find out purpose of v_stop in lane_change (lines: 105 - 128) lc_app_blocked = Behavior("lc_app_blocked", -2) -lc_app_free = Behavior("lc_app_free", convert_to_ms(20)) +lc_app_free = Behavior("lc_app_free", convert_to_ms(30.0)) # Wait lc_wait = Behavior("lc_wait", 0) @@ -63,6 +63,19 @@ def convert_to_ms(speed): lc_exit = Behavior("lc_exit", -1) # Get SpeedLimit dynamically +# Overtake + +# Approach +ot_app_blocked = Behavior("ot_app_blocked", -2) +ot_app_free = Behavior("ot_app_free", -1) +# Wait +ot_wait_stopped = Behavior("ot_wait_stopped", convert_to_ms(0.0)) +ot_wait_free = Behavior("ot_wait_free", -1) +# Enter +ot_enter_init = Behavior("ot_enter_init", -1) +ot_enter_slow = Behavior("ot_enter_slow", -2) +# Exit +ot_leave = Behavior("ot_leave", -1) # Cruise cruise = Behavior("Cruise", -1) diff --git a/code/planning/src/behavior_agent/behaviours/overtake.py b/code/planning/src/behavior_agent/behaviours/overtake.py index 36bc2c36..8c8bf7d5 100644 --- a/code/planning/src/behavior_agent/behaviours/overtake.py +++ b/code/planning/src/behavior_agent/behaviours/overtake.py @@ -2,6 +2,7 @@ from std_msgs.msg import String import rospy +import numpy as np from . import behavior_speed as bs @@ -86,9 +87,9 @@ def update(self): py_trees.common.Status.FAILURE, """ # Update distance to collision object - _dis = self.blackboard.get("/paf/hero/LIDAR_range") + _dis = self.blackboard.get("/paf/hero/collision") if _dis is not None: - self.ot_distance = _dis.data + self.ot_distance = _dis.data[0] rospy.loginfo(f"Overtake distance: {self.ot_distance}") # slow down before overtake if blocked @@ -103,6 +104,7 @@ def update(self): if distance_lidar is not None and \ distance_lidar > 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 else: rospy.loginfo("Overtake blocked slowing down") @@ -229,12 +231,13 @@ def update(self): collision_distance = distance_lidar.data if collision_distance > 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") self.curr_behavior_pub.publish(bs.ot_wait_stopped.name) return py_trees.commom.Status.RUNNING - elif obstacle_msg[1] > convert_to_ms(2): + elif obstacle_msg.data[0] == np.inf: return py_trees.common.Status.FAILURE else: rospy.loginfo("No Lidar Distance") @@ -315,9 +318,21 @@ def update(self): py_trees.common.Status.SUCCESS, py_trees.common.Status.FAILURE, """ - - return py_trees.common.Status.SUCCESS - + status = self.blackboard.get("/paf/hero/overtake_success") + if status is not None: + if status.data == 1: + rospy.loginfo("Overtake: Trajectory planned") + return py_trees.common.Status.SUCCESS + elif status.data == 0: + self.curr_behavior_pub.publish(bs.ot_enter_slow.name) + rospy.loginfo("Overtake: Slowing down") + return py_trees.common.Status.RUNNING + else: + rospy.loginfo("Overtake: Big Failure") + return py_trees.common.Status.FAILURE + else: + rospy.loginfo("Overtake: Bigger Failure") + return py_trees.common.Status.FAILURE # Currently not in use # Can be used to check if we can go back to the original lane diff --git a/code/planning/src/behavior_agent/behaviours/topics2blackboard.py b/code/planning/src/behavior_agent/behaviours/topics2blackboard.py index 86f3c3b1..04e22f59 100755 --- a/code/planning/src/behavior_agent/behaviours/topics2blackboard.py +++ b/code/planning/src/behavior_agent/behaviours/topics2blackboard.py @@ -47,7 +47,9 @@ def create_node(role_name): {'name': f"/paf/{role_name}/collision", 'msg': Float32MultiArray, 'clearing-policy': py_trees.common.ClearingPolicy.ON_INITIALISE}, {'name': f"/paf/{role_name}/current_pos", 'msg': PoseStamped, - 'clearing-policy': py_trees.common.ClearingPolicy.NEVER} + 'clearing-policy': py_trees.common.ClearingPolicy.NEVER}, + {'name': f"/paf/{role_name}/overtake_success", 'msg': Float32, + 'clearing-policy': py_trees.common.ClearingPolicy.NEVER}, ] topics2blackboard = py_trees.composites.Parallel("Topics to Blackboard") diff --git a/code/planning/src/global_planner/global_planner.py b/code/planning/src/global_planner/global_planner.py index 41ee798c..26f806a6 100755 --- a/code/planning/src/global_planner/global_planner.py +++ b/code/planning/src/global_planner/global_planner.py @@ -57,7 +57,7 @@ def __init__(self): # uncomment /paf/hero/global_plan and comment /carla/... for dev_launch self.global_plan_sub = self.new_subscription( msg_type=CarlaRoute, - topic='/carla/' + self.role_name + '/global_plan', + topic='/paf/' + self.role_name + '/global_plan', callback=self.global_route_callback, qos_profile=10) # self.global_plan_sub = self.new_subscription( @@ -84,7 +84,7 @@ def __init__(self): self.logdebug('PrePlanner-Node started') # uncomment for self.dev_load_world_info() for dev_launch - # self.dev_load_world_info() + self.dev_load_world_info() def global_route_callback(self, data: CarlaRoute) -> None: """ @@ -194,7 +194,7 @@ def global_route_callback(self, data: CarlaRoute) -> None: self.path_backup.header.frame_id = "global" self.path_backup.poses = stamped_poses self.path_pub.publish(self.path_backup) - self.loginfo("PrePlanner: published trajectory") + self.logerr("PrePlanner: published trajectory") def world_info_callback(self, opendrive: String) -> None: """ diff --git a/code/planning/src/local_planner/ACC.py b/code/planning/src/local_planner/ACC.py index e09bc575..1c6a388a 100755 --- a/code/planning/src/local_planner/ACC.py +++ b/code/planning/src/local_planner/ACC.py @@ -198,6 +198,9 @@ def loop(timer_event=None): # If we have no obstacle, we want to drive with the current # speed limit self.velocity_pub.publish(self.speed_limit) + else: + self.logerr("ACC: No speed limit recieved") + self.velocity_pub.publish(5.0) self.new_timer(self.control_loop_rate, loop) self.spin() diff --git a/code/planning/src/local_planner/motion_planning.py b/code/planning/src/local_planner/motion_planning.py index e72396b5..97f5bf83 100755 --- a/code/planning/src/local_planner/motion_planning.py +++ b/code/planning/src/local_planner/motion_planning.py @@ -6,7 +6,7 @@ from ros_compatibility.node import CompatibleNode from rospy import Publisher, Subscriber -from std_msgs.msg import String, Float32, Bool +from std_msgs.msg import String, Float32, Bool, Float32MultiArray from nav_msgs.msg import Path from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion from carla_msgs.msg import CarlaSpeedometer @@ -42,6 +42,7 @@ def __init__(self): self.__acc_speed = 0.0 self.__stopline = None # (Distance, isStopline) self.__change_point = None # (Distance, isLaneChange, roadOption) + self.__collision_point = None self.published = False self.current_pos = None self.current_heading = None @@ -53,12 +54,14 @@ def __init__(self): self.current_speed = None self.speed_limit = None - self.counter = 0 - self.speed_list = [] - self.__first_trajectory = None self.__corners = None self.__in_corner = False # Subscriber + self.test_sub = self.new_subscription( + Float32, + f"/paf/{self.role_name}/test", + self.change_trajectory, + qos_profile=1) self.speed_limit_sub = self.new_subscription( Float32, f"/paf/{self.role_name}/speed_limit", @@ -113,6 +116,12 @@ def __init__(self): self.__set_change_point, qos_profile=1) + self.change_point_sub: Subscriber = self.new_subscription( + Float32MultiArray, + f"/paf/{self.role_name}/collision", + self.__set_collision_point, + qos_profile=1) + # Publisher self.traj_pub: Publisher = self.new_publisher( msg_type=Path, @@ -128,10 +137,9 @@ def __init__(self): f"/paf/{self.role_name}/current_wp", self.__set_wp, qos_profile=1) - # Publisher for emergency stop - self.emergency_pub = self.new_publisher( - Bool, - f"/paf/{self.role_name}/emergency", + self.overtake_success_pub = self.new_publisher( + Float32, + f"/paf/{self.role_name}/overtake_success", qos_profile=1) self.logdebug("MotionPlanning started") @@ -169,12 +177,6 @@ def __set_heading(self, data: Float32): """ self.current_heading = data.data - 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 Args: @@ -184,36 +186,47 @@ def __set_current_pos(self, data: PoseStamped): data.pose.position.y, data.pose.position.z]) - def change_trajectory(self, distance: float): + def change_trajectory(self, distance_obj: float): + distance_obj = distance_obj.data self.overtake_start = rospy.get_rostime() limit_waypoints = 30 - np_array = np.array(self.trajectory.poses) - obstacle_position = approx_obstacle_pos(distance, - self.current_heading, - self.current_pos, - self.current_speed) - # trajectory_np = self.convert_pose_to_array(np_array) - # wp = KDTree(trajectory_np[:, :2]).query(obstacle_position[0][:2])[1] - selection = np_array[int(self.current_wp):int(self.current_wp) + - int(distance + limit_waypoints)] - waypoints = self.convert_pose_to_array(selection) - - initial_conditions = { - 'ps': 0, - 'target_speed': self.current_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]]]) - } - result_x, result_y, speeds, ix, iy, iyaw, d, s, speeds_x, \ - speeds_y, misc, costs, success = run_fot(initial_conditions, - hyperparameters) - if success: + pose_list = self.trajectory.poses + count_retrys = 0 + + def calculate_overtake(distance): + obstacle_position = approx_obstacle_pos(distance, + self.current_heading, + self.current_pos, + self.current_speed) + # trajectory_np = self.convert_pose_to_array(pose_list) + # wp=KDTree(trajectory_np[:,:2]).query(obstacle_position[0][:2])[1] + selection = pose_list[int(self.current_wp):int(self.current_wp) + + int(distance + limit_waypoints)] + waypoints = self.convert_pose_to_array(selection) + + initial_conditions = { + 'ps': 0, + 'target_speed': self.current_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) + + 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 = calculate_overtake(distance_obj) + 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) @@ -230,9 +243,14 @@ def change_trajectory(self, distance: float): path = Path() path.header.stamp = rospy.Time.now() path.header.frame_id = "global" - path.poses = list(np_array[:int(self.current_wp)]) + \ - result + list(np_array[int(self.current_wp + 25 + 30):]) + path.poses = pose_list[:int(self.current_wp)] + \ + result + pose_list[int(self.current_wp + + distance_obj + + 30):] self.trajectory = path + else: + self.logerr("Overtake failed") + self.overtake_success_pub.publish(-1) def __set_trajectory(self, data: Path): """get current trajectory global planning @@ -241,9 +259,11 @@ def __set_trajectory(self, data: Path): data (Path): Trajectory waypoints """ self.trajectory = data + self.logerr("Trajectory received") + self.__corners = self.__calc_corner_points() def __calc_corner_points(self): - coords = self.convert_pose_to_array(np.array(self.__first_trajectory)) + coords = self.convert_pose_to_array(np.array(self.trajectory.poses)) x_values = np.array([point[0] for point in coords]) y_values = np.array([point[1] for point in coords]) @@ -285,7 +305,7 @@ def create_sublists(self, points, proximity=5): def get_cornering_speed(self): corner = self.__corners[0] - pos = self.current_pos + pos = self.current_pos[:2] def euclid_dist(vector1, vector2): point1 = np.array(vector1) @@ -358,12 +378,10 @@ 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: - self.target_speed = min(be_speed, acc_speed) + corner_speed = self.get_cornering_speed() + self.target_speed = min(be_speed, acc_speed, corner_speed, 6) 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}") @@ -384,6 +402,10 @@ def __set_change_point(self, data: LaneChange): self.__change_point = \ (data.distance, data.isLaneChange, data.roadOption) + def __set_collision_point(self, data: Float32MultiArray): + if data is not None: + self.__collision_point = data.data[0] + def get_speed_by_behavior(self, behavior: str) -> float: speed = 0.0 split = "_" @@ -392,6 +414,8 @@ def get_speed_by_behavior(self, behavior: str) -> float: speed = self.__get_speed_intersection(behavior) elif short_behavior == "lc": speed = self.__get_speed_lanechange(behavior) + elif short_behavior == "ot": + speed = self.__get_speed_overtake(behavior) elif short_behavior == "parking": speed = bs.parking.speed else: @@ -432,6 +456,25 @@ def __get_speed_lanechange(self, behavior: str) -> float: return speed + def __get_speed_overtake(self, behavior: str) -> float: + speed = 0.0 + 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() + elif behavior == bs.ot_wait_stopped.name: + speed = bs.ot_wait_stopped.speed + elif behavior == bs.ot_wait_free.name: + speed == self.__get_speed_cruise() + elif behavior == bs.ot_enter_init.name: + speed = self.__get_speed_cruise() + 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() + + return speed + def __get_speed_cruise(self) -> float: return self.__acc_speed @@ -461,6 +504,17 @@ def __calc_speed_to_stop_lanechange(self) -> float: v_stop = 0.0 return v_stop + def __calc_speed_to_stop_overtake(self) -> float: + stopline = self.__calc_virtual_overtake() + self.logerr(stopline) + + v_stop = max(convert_to_ms(10.), + convert_to_ms((stopline / 30) + * 50)) + if stopline < 3.0: + v_stop = 0.0 + 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] @@ -473,6 +527,14 @@ def __calc_virtual_stopline(self) -> float: else: return 0.0 + def __calc_virtual_overtake(self) -> float: + self.logerr(f"Test: {self.__collision_point}") + if (self.__collision_point is not None) and \ + self.__collision_point != np.inf: + return self.__collision_point + else: + return 0.0 + def run(self): """ Control loop @@ -483,11 +545,12 @@ def loop(timer_event=None): if (self.__curr_behavior is not None and self.__acc_speed is not None and self.__corners is not None): - self.update_target_speed(self.__acc_speed, - self.__curr_behavior) self.trajectory.header.stamp = rospy.Time.now() self.traj_pub.publish(self.trajectory) - + self.update_target_speed(self.__acc_speed, + self.__curr_behavior) + else: + self.velocity_pub.publish(0.0) self.new_timer(self.control_loop_rate, loop) self.spin() diff --git a/code/test-route/launch/test-route.launch b/code/test-route/launch/test-route.launch index de985c6b..189d9ace 100644 --- a/code/test-route/launch/test-route.launch +++ b/code/test-route/launch/test-route.launch @@ -6,7 +6,7 @@ - + From e2aa73b91a2a98961a7ed04e20b464b32708a54a Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Tue, 30 Jan 2024 16:56:26 +0100 Subject: [PATCH 06/33] feat: change --- code/planning/src/local_planner/motion_planning.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/code/planning/src/local_planner/motion_planning.py b/code/planning/src/local_planner/motion_planning.py index 97f5bf83..2054542c 100755 --- a/code/planning/src/local_planner/motion_planning.py +++ b/code/planning/src/local_planner/motion_planning.py @@ -206,7 +206,7 @@ def calculate_overtake(distance): initial_conditions = { 'ps': 0, - 'target_speed': self.current_speed, + '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]]), From 13dd3933a70b24f86102b18dc0b2040a1d74687a Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Tue, 6 Feb 2024 15:37:51 +0100 Subject: [PATCH 07/33] feat: overtakeing behavior not working (testing ongoing) --- code/planning/launch/planning.launch | 4 +- .../src/behavior_agent/behaviours/overtake.py | 6 +-- .../src/global_planner/global_planner.py | 8 ++-- code/planning/src/local_planner/ACC.py | 13 ++++-- .../src/local_planner/collision_check.py | 42 +++++++++++------- .../src/local_planner/motion_planning.py | 15 +++---- code/planning/src/local_planner/utils.py | 43 ++++++++++++++++++- 7 files changed, 94 insertions(+), 37 deletions(-) diff --git a/code/planning/launch/planning.launch b/code/planning/launch/planning.launch index 72c1f88c..241786f0 100644 --- a/code/planning/launch/planning.launch +++ b/code/planning/launch/planning.launch @@ -1,7 +1,7 @@ - + @@ -13,7 +13,7 @@ --> - + diff --git a/code/planning/src/behavior_agent/behaviours/overtake.py b/code/planning/src/behavior_agent/behaviours/overtake.py index 8c8bf7d5..ca508734 100644 --- a/code/planning/src/behavior_agent/behaviours/overtake.py +++ b/code/planning/src/behavior_agent/behaviours/overtake.py @@ -123,7 +123,7 @@ def update(self): rospy.loginfo("still approaching") return py_trees.common.Status.RUNNING elif speed < convert_to_ms(2.0) and \ - self.ot_distance < 5.0: + self.ot_distance < 6.0: # stopped rospy.loginfo("stopped") return py_trees.common.Status.SUCCESS @@ -218,13 +218,13 @@ def update(self): elif self.ot_option == 1: distance_lidar = self.blackboard. \ get("/carla/hero/LIDAR_range") - distance_lidar = distance_lidar.data clear_distance = 30 else: distance_lidar = None 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: @@ -332,7 +332,7 @@ def update(self): return py_trees.common.Status.FAILURE else: rospy.loginfo("Overtake: Bigger Failure") - return py_trees.common.Status.FAILURE + return py_trees.common.Status.RUNNING # Currently not in use # Can be used to check if we can go back to the original lane diff --git a/code/planning/src/global_planner/global_planner.py b/code/planning/src/global_planner/global_planner.py index 26f806a6..8d8c2743 100755 --- a/code/planning/src/global_planner/global_planner.py +++ b/code/planning/src/global_planner/global_planner.py @@ -121,8 +121,6 @@ def global_route_callback(self, data: CarlaRoute) -> None: self.global_route_backup = data return - self.global_route_backup = None - # get the first turn command (1, 2, or 3) ind = 0 for i, opt in enumerate(data.road_options): @@ -194,6 +192,7 @@ def global_route_callback(self, data: CarlaRoute) -> None: self.path_backup.header.frame_id = "global" self.path_backup.poses = stamped_poses self.path_pub.publish(self.path_backup) + self.global_route_backup = None self.logerr("PrePlanner: published trajectory") def world_info_callback(self, opendrive: String) -> None: @@ -241,7 +240,10 @@ def position_callback(self, data: PoseStamped): if self.global_route_backup is not None: self.loginfo("PrePlanner: Received a pose update retrying " "route preplanning") - self.global_route_callback(self.global_route_backup) + try: + self.global_route_callback(self.global_route_backup) + except Exception: + self.logerr("Preplanner failed -> restart") def dev_load_world_info(self): file_path = \ diff --git a/code/planning/src/local_planner/ACC.py b/code/planning/src/local_planner/ACC.py index 1c6a388a..9ad870ae 100755 --- a/code/planning/src/local_planner/ACC.py +++ b/code/planning/src/local_planner/ACC.py @@ -9,7 +9,7 @@ # from std_msgs.msg import String from std_msgs.msg import Float32MultiArray, Float32 from collision_check import CollisionCheck -import time +import rospy class ACC(CompatibleNode): @@ -109,7 +109,7 @@ def __approx_speed_callback(self, data: Float32): data (Float32): Speed from obstacle in front """ # self.logerr("ACC: Approx speed recieved: " + str(data.data)) - self.obstacle_speed = (time.time(), data.data) + self.obstacle_speed = (rospy.get_rostime(), data.data) def __get_current_velocity(self, data: CarlaSpeedometer): """_summary_ @@ -173,8 +173,10 @@ def loop(timer_event=None): """ if self.obstacle_speed is not None: # Check if too much time has passed since last speed update - if self.obstacle_speed[0] + 0.5 < time.time(): + if rospy.get_rostime() - self.obstacle_speed[0] < \ + rospy.Duration(1): self.obstacle_speed = None + self.obstacle_distance = None if self.obstacle_distance is not None and \ self.obstacle_speed is not None and \ @@ -188,10 +190,14 @@ def loop(timer_event=None): # speed to meet the desired distance safe_speed = self.obstacle_speed[1] * \ (self.obstacle_distance / safety_distance) + if safe_speed < 1.0: + safe_speed = 0 self.velocity_pub.publish(safe_speed) else: # If safety distance is reached, drive with same speed as # Object in front + if self.obstacle_speed[1] < 1.0: + self.obstacle_speed[1] = 0 self.velocity_pub.publish(self.obstacle_speed[1]) elif self.speed_limit is not None: @@ -199,7 +205,6 @@ def loop(timer_event=None): # speed limit self.velocity_pub.publish(self.speed_limit) else: - self.logerr("ACC: No speed limit recieved") self.velocity_pub.publish(5.0) self.new_timer(self.control_loop_rate, loop) diff --git a/code/planning/src/local_planner/collision_check.py b/code/planning/src/local_planner/collision_check.py index 53146d2d..11ad6fa4 100755 --- a/code/planning/src/local_planner/collision_check.py +++ b/code/planning/src/local_planner/collision_check.py @@ -1,6 +1,7 @@ #!/usr/bin/env python # import rospy import numpy as np +import rospy # import tf.transformations import ros_compatibility as roscomp from ros_compatibility.node import CompatibleNode @@ -9,7 +10,6 @@ # from std_msgs.msg import String from std_msgs.msg import Float32, Float32MultiArray from std_msgs.msg import Bool -import time class CollisionCheck(CompatibleNode): @@ -77,7 +77,15 @@ def __set_distance(self, data: Float32): Args: data (Float32): Message from lidar with distance """ - self.__object_last_position = (time.time(), data.data) + 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(), data.data) + self.update_distance() + self.calculate_obstacle_speed() def calculate_obstacle_speed(self): """Caluclate the speed of the obstacle in front of the ego vehicle @@ -91,15 +99,18 @@ def calculate_obstacle_speed(self): # If distance is np.inf no car is in front # Calculate time since last position update - time_difference = self.__object_last_position[0] - \ + rospy_time_difference = self.__object_last_position[0] - \ self.__object_first_position[0] - + time_difference = rospy_time_difference.nsecs/1e9 # Calculate distance (in m) - distance = self.__object_last_position[1] -\ + distance = self.__object_last_position[1] - \ self.__object_first_position[1] + try: + # Speed is distance/time (m/s) + relative_speed = distance/time_difference + except ZeroDivisionError: + return - # Speed is distance/time (m/s) - relative_speed = distance/time_difference speed = self.__current_velocity + relative_speed # Publish speed to ACC for permanent distance check self.speed_publisher.publish(Float32(data=speed)) @@ -180,7 +191,6 @@ def check_crash(self, obstacle): # Initiate emergency brake self.logerr("Emergency Brake") self.emergency_pub.publish(True) - return # When no emergency brake is needed publish collision object data = Float32MultiArray(data=[distance, obstacle_speed]) self.collision_pub.publish(data) @@ -194,14 +204,14 @@ def run(self): Control loop :return: """ - def loop(timer_event=None): - """ - Checks if distance to a possible object is too small and - publishes the desired speed to motion planning - """ - self.update_distance() - self.calculate_obstacle_speed() - self.new_timer(self.control_loop_rate, loop) + # def loop(timer_event=None): + # """ + # Checks if distance to a possible object is too small and + # publishes the desired speed to motion planning + # """ + # self.update_distance() + # self.calculate_obstacle_speed() + # self.new_timer(self.control_loop_rate, loop) self.spin() diff --git a/code/planning/src/local_planner/motion_planning.py b/code/planning/src/local_planner/motion_planning.py index 2054542c..3df23b8c 100755 --- a/code/planning/src/local_planner/motion_planning.py +++ b/code/planning/src/local_planner/motion_planning.py @@ -48,12 +48,10 @@ def __init__(self): self.current_heading = None self.trajectory = None self.overtaking = False - self.overtake_start = rospy.get_rostime() self.current_wp = None self.enhanced_path = None self.current_speed = None self.speed_limit = None - self.__corners = None self.__in_corner = False # Subscriber @@ -186,9 +184,7 @@ def __set_current_pos(self, data: PoseStamped): data.pose.position.y, data.pose.position.z]) - def change_trajectory(self, distance_obj: float): - distance_obj = distance_obj.data - self.overtake_start = rospy.get_rostime() + def change_trajectory(self, distance_obj): limit_waypoints = 30 pose_list = self.trajectory.poses count_retrys = 0 @@ -379,10 +375,11 @@ def update_target_speed(self, acc_speed, behavior): be_speed = self.get_speed_by_behavior(behavior) if not behavior == bs.parking.name: corner_speed = self.get_cornering_speed() - self.target_speed = min(be_speed, acc_speed, corner_speed, 6) + 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.loginfo(f"Speed: {self.target_speed}") self.velocity_pub.publish(self.target_speed) # self.logerr(f"Speed: {self.target_speed}") # self.speed_list.append(self.target_speed) @@ -392,6 +389,8 @@ def __set_acc_speed(self, data: Float32): def __set_curr_behavior(self, data: String): self.__curr_behavior = data.data + if data.data == bs.ot_enter_init.name: + self.change_trajectory(self.__collision_point) def __set_stopline(self, data: Waypoint) -> float: if data is not None: @@ -511,7 +510,7 @@ def __calc_speed_to_stop_overtake(self) -> float: v_stop = max(convert_to_ms(10.), convert_to_ms((stopline / 30) * 50)) - if stopline < 3.0: + if stopline < 6.0: v_stop = 0.0 return v_stop @@ -528,7 +527,7 @@ def __calc_virtual_stopline(self) -> float: return 0.0 def __calc_virtual_overtake(self) -> float: - self.logerr(f"Test: {self.__collision_point}") + self.logerr(f"Overtake point: {self.__collision_point}") if (self.__collision_point is not None) and \ self.__collision_point != np.inf: return self.__collision_point diff --git a/code/planning/src/local_planner/utils.py b/code/planning/src/local_planner/utils.py index dee2fc25..c41ec702 100644 --- a/code/planning/src/local_planner/utils.py +++ b/code/planning/src/local_planner/utils.py @@ -14,7 +14,7 @@ "mint": 6.0, "d_t_s": 0.5, "n_s_sample": 2.0, - "obstacle_clearance": 2, + "obstacle_clearance": 1.5, "kd": 1.0, "kv": 0.1, "ka": 0.1, @@ -112,3 +112,44 @@ def convert_to_ms(speed: float): float: speed in m/s """ return speed / 3.6 + + +def spawn_car(distance): + """Only used for testing, spawns a car in the given distance + + Args: + distance (float): distance + """ + import carla + import os + CARLA_HOST = os.environ.get('CARLA_HOST', 'paf23-carla-simulator-1') + CARLA_PORT = int(os.environ.get('CARLA_PORT', '2000')) + + client = carla.Client(CARLA_HOST, CARLA_PORT) + + world = client.get_world() + world.wait_for_tick() + + blueprint_library = world.get_blueprint_library() + # bp = blueprint_library.filter('vehicle.*')[0] + # vehicle = world.spawn_actor(bp, world.get_map().get_spawn_points()[0]) + bp = blueprint_library.filter("model3")[0] + for actor in world.get_actors(): + if actor.attributes.get('role_name') == "hero": + ego_vehicle = actor + break + + spawnPoint = carla.Transform(ego_vehicle.get_location() + + carla.Location(y=distance), + ego_vehicle.get_transform().rotation) + vehicle = world.spawn_actor(bp, spawnPoint) + + vehicle.set_autopilot(False) + # vehicle.set_location(loc) + # coords = vehicle.get_location() + # get spectator + spectator = world.get_spectator() + # set spectator to follow ego vehicle with offset + spectator.set_transform( + carla.Transform(ego_vehicle.get_location() + carla.Location(z=50), + carla.Rotation(pitch=-90))) From a948e91ed3c3ec0b10a865cc841676acffd2e0cd Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Wed, 6 Mar 2024 13:15:03 +0100 Subject: [PATCH 08/33] feat: cleanup --- code/planning/src/local_planner/collision_check.py | 1 + code/planning/src/local_planner/motion_planning.py | 6 +++--- code/planning/src/local_planner/utils.py | 4 ++-- 3 files changed, 6 insertions(+), 5 deletions(-) diff --git a/code/planning/src/local_planner/collision_check.py b/code/planning/src/local_planner/collision_check.py index 11ad6fa4..ab1047a7 100755 --- a/code/planning/src/local_planner/collision_check.py +++ b/code/planning/src/local_planner/collision_check.py @@ -193,6 +193,7 @@ def check_crash(self, obstacle): self.emergency_pub.publish(True) # When no emergency brake is needed publish collision object data = Float32MultiArray(data=[distance, obstacle_speed]) + self.logerr("Collision published") self.collision_pub.publish(data) else: # If no collision is ahead publish np.Inf diff --git a/code/planning/src/local_planner/motion_planning.py b/code/planning/src/local_planner/motion_planning.py index 3df23b8c..61158bc5 100755 --- a/code/planning/src/local_planner/motion_planning.py +++ b/code/planning/src/local_planner/motion_planning.py @@ -19,7 +19,7 @@ from behavior_agent.behaviours import behavior_speed as bs from utils import convert_to_ms, approx_obstacle_pos, \ - hyperparameters + hyperparameters, spawn_car # from scipy.spatial._kdtree import KDTree @@ -58,7 +58,7 @@ def __init__(self): self.test_sub = self.new_subscription( Float32, f"/paf/{self.role_name}/test", - self.change_trajectory, + spawn_car, qos_profile=1) self.speed_limit_sub = self.new_subscription( Float32, @@ -375,7 +375,7 @@ def update_target_speed(self, acc_speed, behavior): be_speed = self.get_speed_by_behavior(behavior) if not behavior == bs.parking.name: corner_speed = self.get_cornering_speed() - self.target_speed = min(be_speed, acc_speed, corner_speed) + self.target_speed = min(be_speed, acc_speed, corner_speed, 6) else: self.target_speed = be_speed # self.target_speed = min(self.target_speed, 8) diff --git a/code/planning/src/local_planner/utils.py b/code/planning/src/local_planner/utils.py index c41ec702..6a696588 100644 --- a/code/planning/src/local_planner/utils.py +++ b/code/planning/src/local_planner/utils.py @@ -23,7 +23,7 @@ "ko": 0.1, "klat": 1.0, "klon": 1.0, - "num_threads": 1, # set 0 to avoid using threaded algorithm + "num_threads": 3, # set 0 to avoid using threaded algorithm } @@ -140,7 +140,7 @@ def spawn_car(distance): break spawnPoint = carla.Transform(ego_vehicle.get_location() + - carla.Location(y=distance), + carla.Location(y=distance.data), ego_vehicle.get_transform().rotation) vehicle = world.spawn_actor(bp, spawnPoint) From 7b8b38895b0dd9b4a55806823dddc77c1368bf76 Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Tue, 6 Feb 2024 15:37:51 +0100 Subject: [PATCH 09/33] feat: overtakeing behavior not working (testing ongoing) --- code/planning/launch/planning.launch | 4 +- .../src/behavior_agent/behaviours/overtake.py | 6 +-- .../src/global_planner/global_planner.py | 8 ++-- code/planning/src/local_planner/ACC.py | 13 ++++-- .../src/local_planner/collision_check.py | 42 +++++++++++------- .../src/local_planner/motion_planning.py | 15 +++---- code/planning/src/local_planner/utils.py | 43 ++++++++++++++++++- 7 files changed, 94 insertions(+), 37 deletions(-) diff --git a/code/planning/launch/planning.launch b/code/planning/launch/planning.launch index 72c1f88c..241786f0 100644 --- a/code/planning/launch/planning.launch +++ b/code/planning/launch/planning.launch @@ -1,7 +1,7 @@ - + @@ -13,7 +13,7 @@ --> - + diff --git a/code/planning/src/behavior_agent/behaviours/overtake.py b/code/planning/src/behavior_agent/behaviours/overtake.py index 8c8bf7d5..ca508734 100644 --- a/code/planning/src/behavior_agent/behaviours/overtake.py +++ b/code/planning/src/behavior_agent/behaviours/overtake.py @@ -123,7 +123,7 @@ def update(self): rospy.loginfo("still approaching") return py_trees.common.Status.RUNNING elif speed < convert_to_ms(2.0) and \ - self.ot_distance < 5.0: + self.ot_distance < 6.0: # stopped rospy.loginfo("stopped") return py_trees.common.Status.SUCCESS @@ -218,13 +218,13 @@ def update(self): elif self.ot_option == 1: distance_lidar = self.blackboard. \ get("/carla/hero/LIDAR_range") - distance_lidar = distance_lidar.data clear_distance = 30 else: distance_lidar = None 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: @@ -332,7 +332,7 @@ def update(self): return py_trees.common.Status.FAILURE else: rospy.loginfo("Overtake: Bigger Failure") - return py_trees.common.Status.FAILURE + return py_trees.common.Status.RUNNING # Currently not in use # Can be used to check if we can go back to the original lane diff --git a/code/planning/src/global_planner/global_planner.py b/code/planning/src/global_planner/global_planner.py index 26f806a6..8d8c2743 100755 --- a/code/planning/src/global_planner/global_planner.py +++ b/code/planning/src/global_planner/global_planner.py @@ -121,8 +121,6 @@ def global_route_callback(self, data: CarlaRoute) -> None: self.global_route_backup = data return - self.global_route_backup = None - # get the first turn command (1, 2, or 3) ind = 0 for i, opt in enumerate(data.road_options): @@ -194,6 +192,7 @@ def global_route_callback(self, data: CarlaRoute) -> None: self.path_backup.header.frame_id = "global" self.path_backup.poses = stamped_poses self.path_pub.publish(self.path_backup) + self.global_route_backup = None self.logerr("PrePlanner: published trajectory") def world_info_callback(self, opendrive: String) -> None: @@ -241,7 +240,10 @@ def position_callback(self, data: PoseStamped): if self.global_route_backup is not None: self.loginfo("PrePlanner: Received a pose update retrying " "route preplanning") - self.global_route_callback(self.global_route_backup) + try: + self.global_route_callback(self.global_route_backup) + except Exception: + self.logerr("Preplanner failed -> restart") def dev_load_world_info(self): file_path = \ diff --git a/code/planning/src/local_planner/ACC.py b/code/planning/src/local_planner/ACC.py index 1c6a388a..9ad870ae 100755 --- a/code/planning/src/local_planner/ACC.py +++ b/code/planning/src/local_planner/ACC.py @@ -9,7 +9,7 @@ # from std_msgs.msg import String from std_msgs.msg import Float32MultiArray, Float32 from collision_check import CollisionCheck -import time +import rospy class ACC(CompatibleNode): @@ -109,7 +109,7 @@ def __approx_speed_callback(self, data: Float32): data (Float32): Speed from obstacle in front """ # self.logerr("ACC: Approx speed recieved: " + str(data.data)) - self.obstacle_speed = (time.time(), data.data) + self.obstacle_speed = (rospy.get_rostime(), data.data) def __get_current_velocity(self, data: CarlaSpeedometer): """_summary_ @@ -173,8 +173,10 @@ def loop(timer_event=None): """ if self.obstacle_speed is not None: # Check if too much time has passed since last speed update - if self.obstacle_speed[0] + 0.5 < time.time(): + if rospy.get_rostime() - self.obstacle_speed[0] < \ + rospy.Duration(1): self.obstacle_speed = None + self.obstacle_distance = None if self.obstacle_distance is not None and \ self.obstacle_speed is not None and \ @@ -188,10 +190,14 @@ def loop(timer_event=None): # speed to meet the desired distance safe_speed = self.obstacle_speed[1] * \ (self.obstacle_distance / safety_distance) + if safe_speed < 1.0: + safe_speed = 0 self.velocity_pub.publish(safe_speed) else: # If safety distance is reached, drive with same speed as # Object in front + if self.obstacle_speed[1] < 1.0: + self.obstacle_speed[1] = 0 self.velocity_pub.publish(self.obstacle_speed[1]) elif self.speed_limit is not None: @@ -199,7 +205,6 @@ def loop(timer_event=None): # speed limit self.velocity_pub.publish(self.speed_limit) else: - self.logerr("ACC: No speed limit recieved") self.velocity_pub.publish(5.0) self.new_timer(self.control_loop_rate, loop) diff --git a/code/planning/src/local_planner/collision_check.py b/code/planning/src/local_planner/collision_check.py index 53146d2d..11ad6fa4 100755 --- a/code/planning/src/local_planner/collision_check.py +++ b/code/planning/src/local_planner/collision_check.py @@ -1,6 +1,7 @@ #!/usr/bin/env python # import rospy import numpy as np +import rospy # import tf.transformations import ros_compatibility as roscomp from ros_compatibility.node import CompatibleNode @@ -9,7 +10,6 @@ # from std_msgs.msg import String from std_msgs.msg import Float32, Float32MultiArray from std_msgs.msg import Bool -import time class CollisionCheck(CompatibleNode): @@ -77,7 +77,15 @@ def __set_distance(self, data: Float32): Args: data (Float32): Message from lidar with distance """ - self.__object_last_position = (time.time(), data.data) + 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(), data.data) + self.update_distance() + self.calculate_obstacle_speed() def calculate_obstacle_speed(self): """Caluclate the speed of the obstacle in front of the ego vehicle @@ -91,15 +99,18 @@ def calculate_obstacle_speed(self): # If distance is np.inf no car is in front # Calculate time since last position update - time_difference = self.__object_last_position[0] - \ + rospy_time_difference = self.__object_last_position[0] - \ self.__object_first_position[0] - + time_difference = rospy_time_difference.nsecs/1e9 # Calculate distance (in m) - distance = self.__object_last_position[1] -\ + distance = self.__object_last_position[1] - \ self.__object_first_position[1] + try: + # Speed is distance/time (m/s) + relative_speed = distance/time_difference + except ZeroDivisionError: + return - # Speed is distance/time (m/s) - relative_speed = distance/time_difference speed = self.__current_velocity + relative_speed # Publish speed to ACC for permanent distance check self.speed_publisher.publish(Float32(data=speed)) @@ -180,7 +191,6 @@ def check_crash(self, obstacle): # Initiate emergency brake self.logerr("Emergency Brake") self.emergency_pub.publish(True) - return # When no emergency brake is needed publish collision object data = Float32MultiArray(data=[distance, obstacle_speed]) self.collision_pub.publish(data) @@ -194,14 +204,14 @@ def run(self): Control loop :return: """ - def loop(timer_event=None): - """ - Checks if distance to a possible object is too small and - publishes the desired speed to motion planning - """ - self.update_distance() - self.calculate_obstacle_speed() - self.new_timer(self.control_loop_rate, loop) + # def loop(timer_event=None): + # """ + # Checks if distance to a possible object is too small and + # publishes the desired speed to motion planning + # """ + # self.update_distance() + # self.calculate_obstacle_speed() + # self.new_timer(self.control_loop_rate, loop) self.spin() diff --git a/code/planning/src/local_planner/motion_planning.py b/code/planning/src/local_planner/motion_planning.py index 2054542c..3df23b8c 100755 --- a/code/planning/src/local_planner/motion_planning.py +++ b/code/planning/src/local_planner/motion_planning.py @@ -48,12 +48,10 @@ def __init__(self): self.current_heading = None self.trajectory = None self.overtaking = False - self.overtake_start = rospy.get_rostime() self.current_wp = None self.enhanced_path = None self.current_speed = None self.speed_limit = None - self.__corners = None self.__in_corner = False # Subscriber @@ -186,9 +184,7 @@ def __set_current_pos(self, data: PoseStamped): data.pose.position.y, data.pose.position.z]) - def change_trajectory(self, distance_obj: float): - distance_obj = distance_obj.data - self.overtake_start = rospy.get_rostime() + def change_trajectory(self, distance_obj): limit_waypoints = 30 pose_list = self.trajectory.poses count_retrys = 0 @@ -379,10 +375,11 @@ def update_target_speed(self, acc_speed, behavior): be_speed = self.get_speed_by_behavior(behavior) if not behavior == bs.parking.name: corner_speed = self.get_cornering_speed() - self.target_speed = min(be_speed, acc_speed, corner_speed, 6) + 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.loginfo(f"Speed: {self.target_speed}") self.velocity_pub.publish(self.target_speed) # self.logerr(f"Speed: {self.target_speed}") # self.speed_list.append(self.target_speed) @@ -392,6 +389,8 @@ def __set_acc_speed(self, data: Float32): def __set_curr_behavior(self, data: String): self.__curr_behavior = data.data + if data.data == bs.ot_enter_init.name: + self.change_trajectory(self.__collision_point) def __set_stopline(self, data: Waypoint) -> float: if data is not None: @@ -511,7 +510,7 @@ def __calc_speed_to_stop_overtake(self) -> float: v_stop = max(convert_to_ms(10.), convert_to_ms((stopline / 30) * 50)) - if stopline < 3.0: + if stopline < 6.0: v_stop = 0.0 return v_stop @@ -528,7 +527,7 @@ def __calc_virtual_stopline(self) -> float: return 0.0 def __calc_virtual_overtake(self) -> float: - self.logerr(f"Test: {self.__collision_point}") + self.logerr(f"Overtake point: {self.__collision_point}") if (self.__collision_point is not None) and \ self.__collision_point != np.inf: return self.__collision_point diff --git a/code/planning/src/local_planner/utils.py b/code/planning/src/local_planner/utils.py index dee2fc25..c41ec702 100644 --- a/code/planning/src/local_planner/utils.py +++ b/code/planning/src/local_planner/utils.py @@ -14,7 +14,7 @@ "mint": 6.0, "d_t_s": 0.5, "n_s_sample": 2.0, - "obstacle_clearance": 2, + "obstacle_clearance": 1.5, "kd": 1.0, "kv": 0.1, "ka": 0.1, @@ -112,3 +112,44 @@ def convert_to_ms(speed: float): float: speed in m/s """ return speed / 3.6 + + +def spawn_car(distance): + """Only used for testing, spawns a car in the given distance + + Args: + distance (float): distance + """ + import carla + import os + CARLA_HOST = os.environ.get('CARLA_HOST', 'paf23-carla-simulator-1') + CARLA_PORT = int(os.environ.get('CARLA_PORT', '2000')) + + client = carla.Client(CARLA_HOST, CARLA_PORT) + + world = client.get_world() + world.wait_for_tick() + + blueprint_library = world.get_blueprint_library() + # bp = blueprint_library.filter('vehicle.*')[0] + # vehicle = world.spawn_actor(bp, world.get_map().get_spawn_points()[0]) + bp = blueprint_library.filter("model3")[0] + for actor in world.get_actors(): + if actor.attributes.get('role_name') == "hero": + ego_vehicle = actor + break + + spawnPoint = carla.Transform(ego_vehicle.get_location() + + carla.Location(y=distance), + ego_vehicle.get_transform().rotation) + vehicle = world.spawn_actor(bp, spawnPoint) + + vehicle.set_autopilot(False) + # vehicle.set_location(loc) + # coords = vehicle.get_location() + # get spectator + spectator = world.get_spectator() + # set spectator to follow ego vehicle with offset + spectator.set_transform( + carla.Transform(ego_vehicle.get_location() + carla.Location(z=50), + carla.Rotation(pitch=-90))) From 71b802968283fca9519c61d2e0043bc5cf7accd8 Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Wed, 6 Mar 2024 13:15:03 +0100 Subject: [PATCH 10/33] feat: cleanup --- code/planning/src/local_planner/collision_check.py | 1 + code/planning/src/local_planner/motion_planning.py | 6 +++--- code/planning/src/local_planner/utils.py | 4 ++-- 3 files changed, 6 insertions(+), 5 deletions(-) diff --git a/code/planning/src/local_planner/collision_check.py b/code/planning/src/local_planner/collision_check.py index 11ad6fa4..ab1047a7 100755 --- a/code/planning/src/local_planner/collision_check.py +++ b/code/planning/src/local_planner/collision_check.py @@ -193,6 +193,7 @@ def check_crash(self, obstacle): self.emergency_pub.publish(True) # When no emergency brake is needed publish collision object data = Float32MultiArray(data=[distance, obstacle_speed]) + self.logerr("Collision published") self.collision_pub.publish(data) else: # If no collision is ahead publish np.Inf diff --git a/code/planning/src/local_planner/motion_planning.py b/code/planning/src/local_planner/motion_planning.py index 3df23b8c..61158bc5 100755 --- a/code/planning/src/local_planner/motion_planning.py +++ b/code/planning/src/local_planner/motion_planning.py @@ -19,7 +19,7 @@ from behavior_agent.behaviours import behavior_speed as bs from utils import convert_to_ms, approx_obstacle_pos, \ - hyperparameters + hyperparameters, spawn_car # from scipy.spatial._kdtree import KDTree @@ -58,7 +58,7 @@ def __init__(self): self.test_sub = self.new_subscription( Float32, f"/paf/{self.role_name}/test", - self.change_trajectory, + spawn_car, qos_profile=1) self.speed_limit_sub = self.new_subscription( Float32, @@ -375,7 +375,7 @@ def update_target_speed(self, acc_speed, behavior): be_speed = self.get_speed_by_behavior(behavior) if not behavior == bs.parking.name: corner_speed = self.get_cornering_speed() - self.target_speed = min(be_speed, acc_speed, corner_speed) + self.target_speed = min(be_speed, acc_speed, corner_speed, 6) else: self.target_speed = be_speed # self.target_speed = min(self.target_speed, 8) diff --git a/code/planning/src/local_planner/utils.py b/code/planning/src/local_planner/utils.py index c41ec702..6a696588 100644 --- a/code/planning/src/local_planner/utils.py +++ b/code/planning/src/local_planner/utils.py @@ -23,7 +23,7 @@ "ko": 0.1, "klat": 1.0, "klon": 1.0, - "num_threads": 1, # set 0 to avoid using threaded algorithm + "num_threads": 3, # set 0 to avoid using threaded algorithm } @@ -140,7 +140,7 @@ def spawn_car(distance): break spawnPoint = carla.Transform(ego_vehicle.get_location() + - carla.Location(y=distance), + carla.Location(y=distance.data), ego_vehicle.get_transform().rotation) vehicle = world.spawn_actor(bp, spawnPoint) From 942026473d7c7a8fc85e85d8a757c137593623b3 Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Tue, 30 Jan 2024 14:58:57 +0100 Subject: [PATCH 11/33] fix: linter --- .flake8 | 4 +++- .../src/behavior_agent/behaviours/__init__.py | 2 +- .../03_planning/00_paf23/test_traj.py | 3 ++- .../object-detection-model_evaluation/pt.py | 23 +++++++++++++------ .../pylot.py | 9 +++++--- 5 files changed, 28 insertions(+), 13 deletions(-) diff --git a/.flake8 b/.flake8 index 5a3d3cd9..6cce5018 100644 --- a/.flake8 +++ b/.flake8 @@ -2,4 +2,6 @@ exclude= code/planning/src/behavior_agent/behavior_tree.py, code/planning/src/behavior_agent/behaviours/__init__.py, code/planning/src/behavior_agent/behaviours, - code/planning/__init__.py \ No newline at end of file + code/planning/__init__.py, + doc/02_development/templates/template_class_no_comments.py, + doc/02_development/templates/template_class.py \ No newline at end of file diff --git a/code/planning/src/behavior_agent/behaviours/__init__.py b/code/planning/src/behavior_agent/behaviours/__init__.py index 330a8f1d..eb73c823 100755 --- a/code/planning/src/behavior_agent/behaviours/__init__.py +++ b/code/planning/src/behavior_agent/behaviours/__init__.py @@ -1,3 +1,3 @@ -from . import intersection, lane_change, maneuvers, meta, road_features +from . import intersection, lane_change, overtake, maneuvers, meta, road_features from . import topics2blackboard, traffic_objects from . import behavior_speed diff --git a/doc/03_research/03_planning/00_paf23/test_traj.py b/doc/03_research/03_planning/00_paf23/test_traj.py index b4e1165a..97283e6c 100644 --- a/doc/03_research/03_planning/00_paf23/test_traj.py +++ b/doc/03_research/03_planning/00_paf23/test_traj.py @@ -1,4 +1,5 @@ -from frenet_optimal_trajectory_planner.FrenetOptimalTrajectory.fot_wrapper import run_fot +from frenet_optimal_trajectory_planner.FrenetOptimalTrajectory.fot_wrapper \ + import run_fot import numpy as np import matplotlib.pyplot as plt diff --git a/doc/06_perception/experiments/object-detection-model_evaluation/pt.py b/doc/06_perception/experiments/object-detection-model_evaluation/pt.py index 3b8ccc3f..145fbcfe 100644 --- a/doc/06_perception/experiments/object-detection-model_evaluation/pt.py +++ b/doc/06_perception/experiments/object-detection-model_evaluation/pt.py @@ -6,8 +6,11 @@ from time import perf_counter import torch import torchvision -from torchvision.models.detection.faster_rcnn import FasterRCNN_MobileNet_V3_Large_320_FPN_Weights, FasterRCNN_ResNet50_FPN_V2_Weights -from torchvision.models.detection.retinanet import RetinaNet_ResNet50_FPN_V2_Weights +from torchvision.models.detection.faster_rcnn import \ + FasterRCNN_MobileNet_V3_Large_320_FPN_Weights, \ + FasterRCNN_ResNet50_FPN_V2_Weights +from torchvision.models.detection.retinanet import \ + RetinaNet_ResNet50_FPN_V2_Weights from globals import IMAGE_BASE_FOLDER, IMAGES_FOR_TEST from torchvision.utils import draw_bounding_boxes from pathlib import Path @@ -17,7 +20,8 @@ from torchvision.transforms.functional import to_pil_image ALL_MODELS = { - 'fasterrcnn_mobilenet_v3_large_320_fpn': FasterRCNN_MobileNet_V3_Large_320_FPN_Weights, + 'fasterrcnn_mobilenet_v3_large_320_fpn': + FasterRCNN_MobileNet_V3_Large_320_FPN_Weights, 'fasterrcnn_resnet50_fpn_v2': FasterRCNN_ResNet50_FPN_V2_Weights, 'retinanet_resnet50_fpn_v2': RetinaNet_ResNet50_FPN_V2_Weights, } @@ -28,7 +32,9 @@ def load_model(model_name): print('Loading model...', end='') device = torch.device("cuda" if torch.cuda.is_available() else "cpu") weights = ALL_MODELS[model_name].DEFAULT - model = torchvision.models.detection.__dict__[model_name](weights=weights).to(device) + model = torchvision.models.detection.__dict__[model_name]( + weights=weights + ).to(device) model.eval() return model, weights, device @@ -73,18 +79,21 @@ def load_image(image_path, model_weights, device): label_id_offset = -1 - image_np_with_detections = torch.tensor(image_np * 255, dtype=torch.uint8) + image_np_with_detections = torch.tensor(image_np * 255, + dtype=torch.uint8) boxes = result['boxes'] scores = result['scores'] labels = [weights.meta["categories"][i] for i in result['labels']] - box = draw_bounding_boxes(image_np_with_detections[0], boxes, labels, colors='red', width=2) + box = draw_bounding_boxes(image_np_with_detections[0], boxes, labels, + colors='red', width=2) box_img = to_pil_image(box) file_name = Path(image_path).stem plt.figure(figsize=(32, 18)) - plt.title(f'PyTorch - {m} - {p} - {elapsed_time*1000:.0f}ms', fontsize=30) + plt.title(f'PyTorch - {m} - {p} - {elapsed_time*1000:.0f}ms', + fontsize=30) plt.imshow(box_img) plt.savefig(f'{IMAGE_BASE_FOLDER}/result/{file_name}_PT_{m}.jpg') plt.close() diff --git a/doc/06_perception/experiments/object-detection-model_evaluation/pylot.py b/doc/06_perception/experiments/object-detection-model_evaluation/pylot.py index f3a670c6..d59e5e75 100644 --- a/doc/06_perception/experiments/object-detection-model_evaluation/pylot.py +++ b/doc/06_perception/experiments/object-detection-model_evaluation/pylot.py @@ -1,5 +1,6 @@ ''' -Docs: https://www.tensorflow.org/hub/tutorials/tf2_object_detection, https://pylot.readthedocs.io/en/latest/perception.detection.html +Docs: https://www.tensorflow.org/hub/tutorials/tf2_object_detection, +https://pylot.readthedocs.io/en/latest/perception.detection.html ''' from globals import IMAGE_BASE_FOLDER, IMAGES_FOR_TEST @@ -61,7 +62,8 @@ def get_category_index(label_file): with open(label_file, 'r') as f: labels = f.readlines() labels = [label.strip() for label in labels] - category_index = {i: {'id': i, 'name': name} for i, name in enumerate(labels)} + category_index = \ + {i: {'id': i, 'name': name} for i, name in enumerate(labels)} return category_index @@ -114,7 +116,8 @@ def get_category_index(label_file): file_name = Path(image_path).stem plt.figure(figsize=(32, 18)) - plt.title(f'Pylot (TF) - {m} - {p} - {elapsed_time*1000:.0f}ms', fontsize=30) + plt.title(f'Pylot (TF) - {m} - {p} - {elapsed_time*1000:.0f}ms', + fontsize=30) plt.imshow(image_np_with_detections[0]) plt.savefig(f'{IMAGE_BASE_FOLDER}/result/{file_name}_TF_{m}.jpg') plt.close() From b030ae627c315f638539c61d9e466ce8ff46024f Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Tue, 30 Jan 2024 16:54:23 +0100 Subject: [PATCH 12/33] feat: behaviors complete for overtake --- build/docker-compose.yml | 4 +- code/agent/launch/dev.launch | 2 +- .../src/global_plan_distance_publisher.py | 2 +- code/planning/launch/planning.launch | 4 +- .../behaviours/behavior_speed.py | 15 +- .../src/behavior_agent/behaviours/overtake.py | 27 ++- .../behaviours/topics2blackboard.py | 4 +- .../src/global_planner/global_planner.py | 6 +- code/planning/src/local_planner/ACC.py | 3 + .../src/local_planner/motion_planning.py | 169 ++++++++++++------ code/test-route/launch/test-route.launch | 2 +- 11 files changed, 167 insertions(+), 71 deletions(-) diff --git a/build/docker-compose.yml b/build/docker-compose.yml index 1372dfdb..6ba17e3d 100644 --- a/build/docker-compose.yml +++ b/build/docker-compose.yml @@ -62,8 +62,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=paf23-carla-simulator-1 --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=paf23-carla-simulator-1 --track=MAP" logging: driver: "local" diff --git a/code/agent/launch/dev.launch b/code/agent/launch/dev.launch index 071e9813..cc1f148d 100644 --- a/code/agent/launch/dev.launch +++ b/code/agent/launch/dev.launch @@ -32,7 +32,7 @@ - + diff --git a/code/perception/src/global_plan_distance_publisher.py b/code/perception/src/global_plan_distance_publisher.py index 40da367c..4e4cf37b 100755 --- a/code/perception/src/global_plan_distance_publisher.py +++ b/code/perception/src/global_plan_distance_publisher.py @@ -47,7 +47,7 @@ def __init__(self): # Change comment for dev_launch self.global_plan_subscriber = self.new_subscription( CarlaRoute, - "/carla/" + self.role_name + "/global_plan", + "/paf/" + self.role_name + "/global_plan", self.update_global_route, qos_profile=1) # self.global_plan_subscriber = self.new_subscription( diff --git a/code/planning/launch/planning.launch b/code/planning/launch/planning.launch index 9dee1352..72c1f88c 100644 --- a/code/planning/launch/planning.launch +++ b/code/planning/launch/planning.launch @@ -16,13 +16,13 @@ - + diff --git a/code/planning/src/behavior_agent/behaviours/behavior_speed.py b/code/planning/src/behavior_agent/behaviours/behavior_speed.py index df88e762..248edd49 100755 --- a/code/planning/src/behavior_agent/behaviours/behavior_speed.py +++ b/code/planning/src/behavior_agent/behaviours/behavior_speed.py @@ -47,7 +47,7 @@ def convert_to_ms(speed): # TODO: Find out purpose of v_stop in lane_change (lines: 105 - 128) lc_app_blocked = Behavior("lc_app_blocked", -2) -lc_app_free = Behavior("lc_app_free", convert_to_ms(20)) +lc_app_free = Behavior("lc_app_free", convert_to_ms(30.0)) # Wait lc_wait = Behavior("lc_wait", 0) @@ -63,6 +63,19 @@ def convert_to_ms(speed): lc_exit = Behavior("lc_exit", -1) # Get SpeedLimit dynamically +# Overtake + +# Approach +ot_app_blocked = Behavior("ot_app_blocked", -2) +ot_app_free = Behavior("ot_app_free", -1) +# Wait +ot_wait_stopped = Behavior("ot_wait_stopped", convert_to_ms(0.0)) +ot_wait_free = Behavior("ot_wait_free", -1) +# Enter +ot_enter_init = Behavior("ot_enter_init", -1) +ot_enter_slow = Behavior("ot_enter_slow", -2) +# Exit +ot_leave = Behavior("ot_leave", -1) # Cruise cruise = Behavior("Cruise", -1) diff --git a/code/planning/src/behavior_agent/behaviours/overtake.py b/code/planning/src/behavior_agent/behaviours/overtake.py index 36bc2c36..8c8bf7d5 100644 --- a/code/planning/src/behavior_agent/behaviours/overtake.py +++ b/code/planning/src/behavior_agent/behaviours/overtake.py @@ -2,6 +2,7 @@ from std_msgs.msg import String import rospy +import numpy as np from . import behavior_speed as bs @@ -86,9 +87,9 @@ def update(self): py_trees.common.Status.FAILURE, """ # Update distance to collision object - _dis = self.blackboard.get("/paf/hero/LIDAR_range") + _dis = self.blackboard.get("/paf/hero/collision") if _dis is not None: - self.ot_distance = _dis.data + self.ot_distance = _dis.data[0] rospy.loginfo(f"Overtake distance: {self.ot_distance}") # slow down before overtake if blocked @@ -103,6 +104,7 @@ def update(self): if distance_lidar is not None and \ distance_lidar > 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 else: rospy.loginfo("Overtake blocked slowing down") @@ -229,12 +231,13 @@ def update(self): collision_distance = distance_lidar.data if collision_distance > 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") self.curr_behavior_pub.publish(bs.ot_wait_stopped.name) return py_trees.commom.Status.RUNNING - elif obstacle_msg[1] > convert_to_ms(2): + elif obstacle_msg.data[0] == np.inf: return py_trees.common.Status.FAILURE else: rospy.loginfo("No Lidar Distance") @@ -315,9 +318,21 @@ def update(self): py_trees.common.Status.SUCCESS, py_trees.common.Status.FAILURE, """ - - return py_trees.common.Status.SUCCESS - + status = self.blackboard.get("/paf/hero/overtake_success") + if status is not None: + if status.data == 1: + rospy.loginfo("Overtake: Trajectory planned") + return py_trees.common.Status.SUCCESS + elif status.data == 0: + self.curr_behavior_pub.publish(bs.ot_enter_slow.name) + rospy.loginfo("Overtake: Slowing down") + return py_trees.common.Status.RUNNING + else: + rospy.loginfo("Overtake: Big Failure") + return py_trees.common.Status.FAILURE + else: + rospy.loginfo("Overtake: Bigger Failure") + return py_trees.common.Status.FAILURE # Currently not in use # Can be used to check if we can go back to the original lane diff --git a/code/planning/src/behavior_agent/behaviours/topics2blackboard.py b/code/planning/src/behavior_agent/behaviours/topics2blackboard.py index 86f3c3b1..04e22f59 100755 --- a/code/planning/src/behavior_agent/behaviours/topics2blackboard.py +++ b/code/planning/src/behavior_agent/behaviours/topics2blackboard.py @@ -47,7 +47,9 @@ def create_node(role_name): {'name': f"/paf/{role_name}/collision", 'msg': Float32MultiArray, 'clearing-policy': py_trees.common.ClearingPolicy.ON_INITIALISE}, {'name': f"/paf/{role_name}/current_pos", 'msg': PoseStamped, - 'clearing-policy': py_trees.common.ClearingPolicy.NEVER} + 'clearing-policy': py_trees.common.ClearingPolicy.NEVER}, + {'name': f"/paf/{role_name}/overtake_success", 'msg': Float32, + 'clearing-policy': py_trees.common.ClearingPolicy.NEVER}, ] topics2blackboard = py_trees.composites.Parallel("Topics to Blackboard") diff --git a/code/planning/src/global_planner/global_planner.py b/code/planning/src/global_planner/global_planner.py index 41ee798c..26f806a6 100755 --- a/code/planning/src/global_planner/global_planner.py +++ b/code/planning/src/global_planner/global_planner.py @@ -57,7 +57,7 @@ def __init__(self): # uncomment /paf/hero/global_plan and comment /carla/... for dev_launch self.global_plan_sub = self.new_subscription( msg_type=CarlaRoute, - topic='/carla/' + self.role_name + '/global_plan', + topic='/paf/' + self.role_name + '/global_plan', callback=self.global_route_callback, qos_profile=10) # self.global_plan_sub = self.new_subscription( @@ -84,7 +84,7 @@ def __init__(self): self.logdebug('PrePlanner-Node started') # uncomment for self.dev_load_world_info() for dev_launch - # self.dev_load_world_info() + self.dev_load_world_info() def global_route_callback(self, data: CarlaRoute) -> None: """ @@ -194,7 +194,7 @@ def global_route_callback(self, data: CarlaRoute) -> None: self.path_backup.header.frame_id = "global" self.path_backup.poses = stamped_poses self.path_pub.publish(self.path_backup) - self.loginfo("PrePlanner: published trajectory") + self.logerr("PrePlanner: published trajectory") def world_info_callback(self, opendrive: String) -> None: """ diff --git a/code/planning/src/local_planner/ACC.py b/code/planning/src/local_planner/ACC.py index e09bc575..1c6a388a 100755 --- a/code/planning/src/local_planner/ACC.py +++ b/code/planning/src/local_planner/ACC.py @@ -198,6 +198,9 @@ def loop(timer_event=None): # If we have no obstacle, we want to drive with the current # speed limit self.velocity_pub.publish(self.speed_limit) + else: + self.logerr("ACC: No speed limit recieved") + self.velocity_pub.publish(5.0) self.new_timer(self.control_loop_rate, loop) self.spin() diff --git a/code/planning/src/local_planner/motion_planning.py b/code/planning/src/local_planner/motion_planning.py index e72396b5..97f5bf83 100755 --- a/code/planning/src/local_planner/motion_planning.py +++ b/code/planning/src/local_planner/motion_planning.py @@ -6,7 +6,7 @@ from ros_compatibility.node import CompatibleNode from rospy import Publisher, Subscriber -from std_msgs.msg import String, Float32, Bool +from std_msgs.msg import String, Float32, Bool, Float32MultiArray from nav_msgs.msg import Path from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion from carla_msgs.msg import CarlaSpeedometer @@ -42,6 +42,7 @@ def __init__(self): self.__acc_speed = 0.0 self.__stopline = None # (Distance, isStopline) self.__change_point = None # (Distance, isLaneChange, roadOption) + self.__collision_point = None self.published = False self.current_pos = None self.current_heading = None @@ -53,12 +54,14 @@ def __init__(self): self.current_speed = None self.speed_limit = None - self.counter = 0 - self.speed_list = [] - self.__first_trajectory = None self.__corners = None self.__in_corner = False # Subscriber + self.test_sub = self.new_subscription( + Float32, + f"/paf/{self.role_name}/test", + self.change_trajectory, + qos_profile=1) self.speed_limit_sub = self.new_subscription( Float32, f"/paf/{self.role_name}/speed_limit", @@ -113,6 +116,12 @@ def __init__(self): self.__set_change_point, qos_profile=1) + self.change_point_sub: Subscriber = self.new_subscription( + Float32MultiArray, + f"/paf/{self.role_name}/collision", + self.__set_collision_point, + qos_profile=1) + # Publisher self.traj_pub: Publisher = self.new_publisher( msg_type=Path, @@ -128,10 +137,9 @@ def __init__(self): f"/paf/{self.role_name}/current_wp", self.__set_wp, qos_profile=1) - # Publisher for emergency stop - self.emergency_pub = self.new_publisher( - Bool, - f"/paf/{self.role_name}/emergency", + self.overtake_success_pub = self.new_publisher( + Float32, + f"/paf/{self.role_name}/overtake_success", qos_profile=1) self.logdebug("MotionPlanning started") @@ -169,12 +177,6 @@ def __set_heading(self, data: Float32): """ self.current_heading = data.data - 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 Args: @@ -184,36 +186,47 @@ def __set_current_pos(self, data: PoseStamped): data.pose.position.y, data.pose.position.z]) - def change_trajectory(self, distance: float): + def change_trajectory(self, distance_obj: float): + distance_obj = distance_obj.data self.overtake_start = rospy.get_rostime() limit_waypoints = 30 - np_array = np.array(self.trajectory.poses) - obstacle_position = approx_obstacle_pos(distance, - self.current_heading, - self.current_pos, - self.current_speed) - # trajectory_np = self.convert_pose_to_array(np_array) - # wp = KDTree(trajectory_np[:, :2]).query(obstacle_position[0][:2])[1] - selection = np_array[int(self.current_wp):int(self.current_wp) + - int(distance + limit_waypoints)] - waypoints = self.convert_pose_to_array(selection) - - initial_conditions = { - 'ps': 0, - 'target_speed': self.current_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]]]) - } - result_x, result_y, speeds, ix, iy, iyaw, d, s, speeds_x, \ - speeds_y, misc, costs, success = run_fot(initial_conditions, - hyperparameters) - if success: + pose_list = self.trajectory.poses + count_retrys = 0 + + def calculate_overtake(distance): + obstacle_position = approx_obstacle_pos(distance, + self.current_heading, + self.current_pos, + self.current_speed) + # trajectory_np = self.convert_pose_to_array(pose_list) + # wp=KDTree(trajectory_np[:,:2]).query(obstacle_position[0][:2])[1] + selection = pose_list[int(self.current_wp):int(self.current_wp) + + int(distance + limit_waypoints)] + waypoints = self.convert_pose_to_array(selection) + + initial_conditions = { + 'ps': 0, + 'target_speed': self.current_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) + + 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 = calculate_overtake(distance_obj) + 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) @@ -230,9 +243,14 @@ def change_trajectory(self, distance: float): path = Path() path.header.stamp = rospy.Time.now() path.header.frame_id = "global" - path.poses = list(np_array[:int(self.current_wp)]) + \ - result + list(np_array[int(self.current_wp + 25 + 30):]) + path.poses = pose_list[:int(self.current_wp)] + \ + result + pose_list[int(self.current_wp + + distance_obj + + 30):] self.trajectory = path + else: + self.logerr("Overtake failed") + self.overtake_success_pub.publish(-1) def __set_trajectory(self, data: Path): """get current trajectory global planning @@ -241,9 +259,11 @@ def __set_trajectory(self, data: Path): data (Path): Trajectory waypoints """ self.trajectory = data + self.logerr("Trajectory received") + self.__corners = self.__calc_corner_points() def __calc_corner_points(self): - coords = self.convert_pose_to_array(np.array(self.__first_trajectory)) + coords = self.convert_pose_to_array(np.array(self.trajectory.poses)) x_values = np.array([point[0] for point in coords]) y_values = np.array([point[1] for point in coords]) @@ -285,7 +305,7 @@ def create_sublists(self, points, proximity=5): def get_cornering_speed(self): corner = self.__corners[0] - pos = self.current_pos + pos = self.current_pos[:2] def euclid_dist(vector1, vector2): point1 = np.array(vector1) @@ -358,12 +378,10 @@ 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: - self.target_speed = min(be_speed, acc_speed) + corner_speed = self.get_cornering_speed() + self.target_speed = min(be_speed, acc_speed, corner_speed, 6) 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}") @@ -384,6 +402,10 @@ def __set_change_point(self, data: LaneChange): self.__change_point = \ (data.distance, data.isLaneChange, data.roadOption) + def __set_collision_point(self, data: Float32MultiArray): + if data is not None: + self.__collision_point = data.data[0] + def get_speed_by_behavior(self, behavior: str) -> float: speed = 0.0 split = "_" @@ -392,6 +414,8 @@ def get_speed_by_behavior(self, behavior: str) -> float: speed = self.__get_speed_intersection(behavior) elif short_behavior == "lc": speed = self.__get_speed_lanechange(behavior) + elif short_behavior == "ot": + speed = self.__get_speed_overtake(behavior) elif short_behavior == "parking": speed = bs.parking.speed else: @@ -432,6 +456,25 @@ def __get_speed_lanechange(self, behavior: str) -> float: return speed + def __get_speed_overtake(self, behavior: str) -> float: + speed = 0.0 + 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() + elif behavior == bs.ot_wait_stopped.name: + speed = bs.ot_wait_stopped.speed + elif behavior == bs.ot_wait_free.name: + speed == self.__get_speed_cruise() + elif behavior == bs.ot_enter_init.name: + speed = self.__get_speed_cruise() + 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() + + return speed + def __get_speed_cruise(self) -> float: return self.__acc_speed @@ -461,6 +504,17 @@ def __calc_speed_to_stop_lanechange(self) -> float: v_stop = 0.0 return v_stop + def __calc_speed_to_stop_overtake(self) -> float: + stopline = self.__calc_virtual_overtake() + self.logerr(stopline) + + v_stop = max(convert_to_ms(10.), + convert_to_ms((stopline / 30) + * 50)) + if stopline < 3.0: + v_stop = 0.0 + 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] @@ -473,6 +527,14 @@ def __calc_virtual_stopline(self) -> float: else: return 0.0 + def __calc_virtual_overtake(self) -> float: + self.logerr(f"Test: {self.__collision_point}") + if (self.__collision_point is not None) and \ + self.__collision_point != np.inf: + return self.__collision_point + else: + return 0.0 + def run(self): """ Control loop @@ -483,11 +545,12 @@ def loop(timer_event=None): if (self.__curr_behavior is not None and self.__acc_speed is not None and self.__corners is not None): - self.update_target_speed(self.__acc_speed, - self.__curr_behavior) self.trajectory.header.stamp = rospy.Time.now() self.traj_pub.publish(self.trajectory) - + self.update_target_speed(self.__acc_speed, + self.__curr_behavior) + else: + self.velocity_pub.publish(0.0) self.new_timer(self.control_loop_rate, loop) self.spin() diff --git a/code/test-route/launch/test-route.launch b/code/test-route/launch/test-route.launch index de985c6b..189d9ace 100644 --- a/code/test-route/launch/test-route.launch +++ b/code/test-route/launch/test-route.launch @@ -6,7 +6,7 @@ - + From 3213ad8ec84a0780d6e3f8dddb0f3e2c7435a78c Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Tue, 30 Jan 2024 16:56:26 +0100 Subject: [PATCH 13/33] feat: change --- code/planning/src/local_planner/motion_planning.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/code/planning/src/local_planner/motion_planning.py b/code/planning/src/local_planner/motion_planning.py index 97f5bf83..2054542c 100755 --- a/code/planning/src/local_planner/motion_planning.py +++ b/code/planning/src/local_planner/motion_planning.py @@ -206,7 +206,7 @@ def calculate_overtake(distance): initial_conditions = { 'ps': 0, - 'target_speed': self.current_speed, + '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]]), From 8fb157b12ef0056536531061ef815fac55b8e6c0 Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Tue, 6 Feb 2024 15:37:51 +0100 Subject: [PATCH 14/33] feat: overtakeing behavior not working (testing ongoing) --- code/planning/launch/planning.launch | 4 +- .../src/behavior_agent/behaviours/overtake.py | 6 +-- .../src/global_planner/global_planner.py | 8 ++-- code/planning/src/local_planner/ACC.py | 13 ++++-- .../src/local_planner/collision_check.py | 42 +++++++++++------- .../src/local_planner/motion_planning.py | 15 +++---- code/planning/src/local_planner/utils.py | 43 ++++++++++++++++++- 7 files changed, 94 insertions(+), 37 deletions(-) diff --git a/code/planning/launch/planning.launch b/code/planning/launch/planning.launch index 72c1f88c..241786f0 100644 --- a/code/planning/launch/planning.launch +++ b/code/planning/launch/planning.launch @@ -1,7 +1,7 @@ - + @@ -13,7 +13,7 @@ --> - + diff --git a/code/planning/src/behavior_agent/behaviours/overtake.py b/code/planning/src/behavior_agent/behaviours/overtake.py index 8c8bf7d5..ca508734 100644 --- a/code/planning/src/behavior_agent/behaviours/overtake.py +++ b/code/planning/src/behavior_agent/behaviours/overtake.py @@ -123,7 +123,7 @@ def update(self): rospy.loginfo("still approaching") return py_trees.common.Status.RUNNING elif speed < convert_to_ms(2.0) and \ - self.ot_distance < 5.0: + self.ot_distance < 6.0: # stopped rospy.loginfo("stopped") return py_trees.common.Status.SUCCESS @@ -218,13 +218,13 @@ def update(self): elif self.ot_option == 1: distance_lidar = self.blackboard. \ get("/carla/hero/LIDAR_range") - distance_lidar = distance_lidar.data clear_distance = 30 else: distance_lidar = None 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: @@ -332,7 +332,7 @@ def update(self): return py_trees.common.Status.FAILURE else: rospy.loginfo("Overtake: Bigger Failure") - return py_trees.common.Status.FAILURE + return py_trees.common.Status.RUNNING # Currently not in use # Can be used to check if we can go back to the original lane diff --git a/code/planning/src/global_planner/global_planner.py b/code/planning/src/global_planner/global_planner.py index 26f806a6..8d8c2743 100755 --- a/code/planning/src/global_planner/global_planner.py +++ b/code/planning/src/global_planner/global_planner.py @@ -121,8 +121,6 @@ def global_route_callback(self, data: CarlaRoute) -> None: self.global_route_backup = data return - self.global_route_backup = None - # get the first turn command (1, 2, or 3) ind = 0 for i, opt in enumerate(data.road_options): @@ -194,6 +192,7 @@ def global_route_callback(self, data: CarlaRoute) -> None: self.path_backup.header.frame_id = "global" self.path_backup.poses = stamped_poses self.path_pub.publish(self.path_backup) + self.global_route_backup = None self.logerr("PrePlanner: published trajectory") def world_info_callback(self, opendrive: String) -> None: @@ -241,7 +240,10 @@ def position_callback(self, data: PoseStamped): if self.global_route_backup is not None: self.loginfo("PrePlanner: Received a pose update retrying " "route preplanning") - self.global_route_callback(self.global_route_backup) + try: + self.global_route_callback(self.global_route_backup) + except Exception: + self.logerr("Preplanner failed -> restart") def dev_load_world_info(self): file_path = \ diff --git a/code/planning/src/local_planner/ACC.py b/code/planning/src/local_planner/ACC.py index 1c6a388a..9ad870ae 100755 --- a/code/planning/src/local_planner/ACC.py +++ b/code/planning/src/local_planner/ACC.py @@ -9,7 +9,7 @@ # from std_msgs.msg import String from std_msgs.msg import Float32MultiArray, Float32 from collision_check import CollisionCheck -import time +import rospy class ACC(CompatibleNode): @@ -109,7 +109,7 @@ def __approx_speed_callback(self, data: Float32): data (Float32): Speed from obstacle in front """ # self.logerr("ACC: Approx speed recieved: " + str(data.data)) - self.obstacle_speed = (time.time(), data.data) + self.obstacle_speed = (rospy.get_rostime(), data.data) def __get_current_velocity(self, data: CarlaSpeedometer): """_summary_ @@ -173,8 +173,10 @@ def loop(timer_event=None): """ if self.obstacle_speed is not None: # Check if too much time has passed since last speed update - if self.obstacle_speed[0] + 0.5 < time.time(): + if rospy.get_rostime() - self.obstacle_speed[0] < \ + rospy.Duration(1): self.obstacle_speed = None + self.obstacle_distance = None if self.obstacle_distance is not None and \ self.obstacle_speed is not None and \ @@ -188,10 +190,14 @@ def loop(timer_event=None): # speed to meet the desired distance safe_speed = self.obstacle_speed[1] * \ (self.obstacle_distance / safety_distance) + if safe_speed < 1.0: + safe_speed = 0 self.velocity_pub.publish(safe_speed) else: # If safety distance is reached, drive with same speed as # Object in front + if self.obstacle_speed[1] < 1.0: + self.obstacle_speed[1] = 0 self.velocity_pub.publish(self.obstacle_speed[1]) elif self.speed_limit is not None: @@ -199,7 +205,6 @@ def loop(timer_event=None): # speed limit self.velocity_pub.publish(self.speed_limit) else: - self.logerr("ACC: No speed limit recieved") self.velocity_pub.publish(5.0) self.new_timer(self.control_loop_rate, loop) diff --git a/code/planning/src/local_planner/collision_check.py b/code/planning/src/local_planner/collision_check.py index 53146d2d..11ad6fa4 100755 --- a/code/planning/src/local_planner/collision_check.py +++ b/code/planning/src/local_planner/collision_check.py @@ -1,6 +1,7 @@ #!/usr/bin/env python # import rospy import numpy as np +import rospy # import tf.transformations import ros_compatibility as roscomp from ros_compatibility.node import CompatibleNode @@ -9,7 +10,6 @@ # from std_msgs.msg import String from std_msgs.msg import Float32, Float32MultiArray from std_msgs.msg import Bool -import time class CollisionCheck(CompatibleNode): @@ -77,7 +77,15 @@ def __set_distance(self, data: Float32): Args: data (Float32): Message from lidar with distance """ - self.__object_last_position = (time.time(), data.data) + 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(), data.data) + self.update_distance() + self.calculate_obstacle_speed() def calculate_obstacle_speed(self): """Caluclate the speed of the obstacle in front of the ego vehicle @@ -91,15 +99,18 @@ def calculate_obstacle_speed(self): # If distance is np.inf no car is in front # Calculate time since last position update - time_difference = self.__object_last_position[0] - \ + rospy_time_difference = self.__object_last_position[0] - \ self.__object_first_position[0] - + time_difference = rospy_time_difference.nsecs/1e9 # Calculate distance (in m) - distance = self.__object_last_position[1] -\ + distance = self.__object_last_position[1] - \ self.__object_first_position[1] + try: + # Speed is distance/time (m/s) + relative_speed = distance/time_difference + except ZeroDivisionError: + return - # Speed is distance/time (m/s) - relative_speed = distance/time_difference speed = self.__current_velocity + relative_speed # Publish speed to ACC for permanent distance check self.speed_publisher.publish(Float32(data=speed)) @@ -180,7 +191,6 @@ def check_crash(self, obstacle): # Initiate emergency brake self.logerr("Emergency Brake") self.emergency_pub.publish(True) - return # When no emergency brake is needed publish collision object data = Float32MultiArray(data=[distance, obstacle_speed]) self.collision_pub.publish(data) @@ -194,14 +204,14 @@ def run(self): Control loop :return: """ - def loop(timer_event=None): - """ - Checks if distance to a possible object is too small and - publishes the desired speed to motion planning - """ - self.update_distance() - self.calculate_obstacle_speed() - self.new_timer(self.control_loop_rate, loop) + # def loop(timer_event=None): + # """ + # Checks if distance to a possible object is too small and + # publishes the desired speed to motion planning + # """ + # self.update_distance() + # self.calculate_obstacle_speed() + # self.new_timer(self.control_loop_rate, loop) self.spin() diff --git a/code/planning/src/local_planner/motion_planning.py b/code/planning/src/local_planner/motion_planning.py index 2054542c..3df23b8c 100755 --- a/code/planning/src/local_planner/motion_planning.py +++ b/code/planning/src/local_planner/motion_planning.py @@ -48,12 +48,10 @@ def __init__(self): self.current_heading = None self.trajectory = None self.overtaking = False - self.overtake_start = rospy.get_rostime() self.current_wp = None self.enhanced_path = None self.current_speed = None self.speed_limit = None - self.__corners = None self.__in_corner = False # Subscriber @@ -186,9 +184,7 @@ def __set_current_pos(self, data: PoseStamped): data.pose.position.y, data.pose.position.z]) - def change_trajectory(self, distance_obj: float): - distance_obj = distance_obj.data - self.overtake_start = rospy.get_rostime() + def change_trajectory(self, distance_obj): limit_waypoints = 30 pose_list = self.trajectory.poses count_retrys = 0 @@ -379,10 +375,11 @@ def update_target_speed(self, acc_speed, behavior): be_speed = self.get_speed_by_behavior(behavior) if not behavior == bs.parking.name: corner_speed = self.get_cornering_speed() - self.target_speed = min(be_speed, acc_speed, corner_speed, 6) + 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.loginfo(f"Speed: {self.target_speed}") self.velocity_pub.publish(self.target_speed) # self.logerr(f"Speed: {self.target_speed}") # self.speed_list.append(self.target_speed) @@ -392,6 +389,8 @@ def __set_acc_speed(self, data: Float32): def __set_curr_behavior(self, data: String): self.__curr_behavior = data.data + if data.data == bs.ot_enter_init.name: + self.change_trajectory(self.__collision_point) def __set_stopline(self, data: Waypoint) -> float: if data is not None: @@ -511,7 +510,7 @@ def __calc_speed_to_stop_overtake(self) -> float: v_stop = max(convert_to_ms(10.), convert_to_ms((stopline / 30) * 50)) - if stopline < 3.0: + if stopline < 6.0: v_stop = 0.0 return v_stop @@ -528,7 +527,7 @@ def __calc_virtual_stopline(self) -> float: return 0.0 def __calc_virtual_overtake(self) -> float: - self.logerr(f"Test: {self.__collision_point}") + self.logerr(f"Overtake point: {self.__collision_point}") if (self.__collision_point is not None) and \ self.__collision_point != np.inf: return self.__collision_point diff --git a/code/planning/src/local_planner/utils.py b/code/planning/src/local_planner/utils.py index dee2fc25..c41ec702 100644 --- a/code/planning/src/local_planner/utils.py +++ b/code/planning/src/local_planner/utils.py @@ -14,7 +14,7 @@ "mint": 6.0, "d_t_s": 0.5, "n_s_sample": 2.0, - "obstacle_clearance": 2, + "obstacle_clearance": 1.5, "kd": 1.0, "kv": 0.1, "ka": 0.1, @@ -112,3 +112,44 @@ def convert_to_ms(speed: float): float: speed in m/s """ return speed / 3.6 + + +def spawn_car(distance): + """Only used for testing, spawns a car in the given distance + + Args: + distance (float): distance + """ + import carla + import os + CARLA_HOST = os.environ.get('CARLA_HOST', 'paf23-carla-simulator-1') + CARLA_PORT = int(os.environ.get('CARLA_PORT', '2000')) + + client = carla.Client(CARLA_HOST, CARLA_PORT) + + world = client.get_world() + world.wait_for_tick() + + blueprint_library = world.get_blueprint_library() + # bp = blueprint_library.filter('vehicle.*')[0] + # vehicle = world.spawn_actor(bp, world.get_map().get_spawn_points()[0]) + bp = blueprint_library.filter("model3")[0] + for actor in world.get_actors(): + if actor.attributes.get('role_name') == "hero": + ego_vehicle = actor + break + + spawnPoint = carla.Transform(ego_vehicle.get_location() + + carla.Location(y=distance), + ego_vehicle.get_transform().rotation) + vehicle = world.spawn_actor(bp, spawnPoint) + + vehicle.set_autopilot(False) + # vehicle.set_location(loc) + # coords = vehicle.get_location() + # get spectator + spectator = world.get_spectator() + # set spectator to follow ego vehicle with offset + spectator.set_transform( + carla.Transform(ego_vehicle.get_location() + carla.Location(z=50), + carla.Rotation(pitch=-90))) From 0c9c6bd9d1def514993e8fbf7cc7249cad201925 Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Wed, 6 Mar 2024 13:15:03 +0100 Subject: [PATCH 15/33] feat: cleanup --- code/planning/src/local_planner/collision_check.py | 1 + code/planning/src/local_planner/motion_planning.py | 6 +++--- code/planning/src/local_planner/utils.py | 4 ++-- 3 files changed, 6 insertions(+), 5 deletions(-) diff --git a/code/planning/src/local_planner/collision_check.py b/code/planning/src/local_planner/collision_check.py index 11ad6fa4..ab1047a7 100755 --- a/code/planning/src/local_planner/collision_check.py +++ b/code/planning/src/local_planner/collision_check.py @@ -193,6 +193,7 @@ def check_crash(self, obstacle): self.emergency_pub.publish(True) # When no emergency brake is needed publish collision object data = Float32MultiArray(data=[distance, obstacle_speed]) + self.logerr("Collision published") self.collision_pub.publish(data) else: # If no collision is ahead publish np.Inf diff --git a/code/planning/src/local_planner/motion_planning.py b/code/planning/src/local_planner/motion_planning.py index 3df23b8c..61158bc5 100755 --- a/code/planning/src/local_planner/motion_planning.py +++ b/code/planning/src/local_planner/motion_planning.py @@ -19,7 +19,7 @@ from behavior_agent.behaviours import behavior_speed as bs from utils import convert_to_ms, approx_obstacle_pos, \ - hyperparameters + hyperparameters, spawn_car # from scipy.spatial._kdtree import KDTree @@ -58,7 +58,7 @@ def __init__(self): self.test_sub = self.new_subscription( Float32, f"/paf/{self.role_name}/test", - self.change_trajectory, + spawn_car, qos_profile=1) self.speed_limit_sub = self.new_subscription( Float32, @@ -375,7 +375,7 @@ def update_target_speed(self, acc_speed, behavior): be_speed = self.get_speed_by_behavior(behavior) if not behavior == bs.parking.name: corner_speed = self.get_cornering_speed() - self.target_speed = min(be_speed, acc_speed, corner_speed) + self.target_speed = min(be_speed, acc_speed, corner_speed, 6) else: self.target_speed = be_speed # self.target_speed = min(self.target_speed, 8) diff --git a/code/planning/src/local_planner/utils.py b/code/planning/src/local_planner/utils.py index c41ec702..6a696588 100644 --- a/code/planning/src/local_planner/utils.py +++ b/code/planning/src/local_planner/utils.py @@ -23,7 +23,7 @@ "ko": 0.1, "klat": 1.0, "klon": 1.0, - "num_threads": 1, # set 0 to avoid using threaded algorithm + "num_threads": 3, # set 0 to avoid using threaded algorithm } @@ -140,7 +140,7 @@ def spawn_car(distance): break spawnPoint = carla.Transform(ego_vehicle.get_location() + - carla.Location(y=distance), + carla.Location(y=distance.data), ego_vehicle.get_transform().rotation) vehicle = world.spawn_actor(bp, spawnPoint) From 9a63db97f25d07eeec1d0baaf9e33353d2fc2985 Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Tue, 30 Jan 2024 16:54:23 +0100 Subject: [PATCH 16/33] feat: behaviors complete for overtake --- build/docker-compose.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/build/docker-compose.yml b/build/docker-compose.yml index 6ba17e3d..1372dfdb 100644 --- a/build/docker-compose.yml +++ b/build/docker-compose.yml @@ -62,8 +62,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=paf23-carla-simulator-1 --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=paf23-carla-simulator-1 --track=MAP" logging: driver: "local" From 2c4c433ed6d7fde388d7eba3b0bd0de2ee6bc626 Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Tue, 6 Feb 2024 15:37:51 +0100 Subject: [PATCH 17/33] feat: overtakeing behavior not working (testing ongoing) --- code/planning/src/local_planner/motion_planning.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/code/planning/src/local_planner/motion_planning.py b/code/planning/src/local_planner/motion_planning.py index 61158bc5..e1be4994 100755 --- a/code/planning/src/local_planner/motion_planning.py +++ b/code/planning/src/local_planner/motion_planning.py @@ -375,7 +375,7 @@ def update_target_speed(self, acc_speed, behavior): be_speed = self.get_speed_by_behavior(behavior) if not behavior == bs.parking.name: corner_speed = self.get_cornering_speed() - self.target_speed = min(be_speed, acc_speed, corner_speed, 6) + self.target_speed = min(be_speed, acc_speed, corner_speed) else: self.target_speed = be_speed # self.target_speed = min(self.target_speed, 8) From 6ddcdb38bb63d9bbf99cb2f9a3548a6a001cf138 Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Wed, 6 Mar 2024 13:15:03 +0100 Subject: [PATCH 18/33] feat: cleanup --- code/planning/src/local_planner/motion_planning.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/code/planning/src/local_planner/motion_planning.py b/code/planning/src/local_planner/motion_planning.py index e1be4994..61158bc5 100755 --- a/code/planning/src/local_planner/motion_planning.py +++ b/code/planning/src/local_planner/motion_planning.py @@ -375,7 +375,7 @@ def update_target_speed(self, acc_speed, behavior): be_speed = self.get_speed_by_behavior(behavior) if not behavior == bs.parking.name: corner_speed = self.get_cornering_speed() - self.target_speed = min(be_speed, acc_speed, corner_speed) + self.target_speed = min(be_speed, acc_speed, corner_speed, 6) else: self.target_speed = be_speed # self.target_speed = min(self.target_speed, 8) From 89254abb5ee8e8a4725a43ba77c3e25dc3310ff2 Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Wed, 6 Mar 2024 14:33:16 +0100 Subject: [PATCH 19/33] fix: netwrokx error --- code/requirements.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/code/requirements.txt b/code/requirements.txt index b4374b74..f7e87468 100644 --- a/code/requirements.txt +++ b/code/requirements.txt @@ -15,3 +15,4 @@ numpy==1.23.5 ultralytics==8.0.220 scikit-learn>=0.18 pandas==2.0.0 +networkx==2.4 \ No newline at end of file From 18c32ba189ad63d112d558e590d7da12204afcde Mon Sep 17 00:00:00 2001 From: Leon Okrusch Date: Thu, 7 Mar 2024 13:50:33 +0100 Subject: [PATCH 20/33] feat(192): visionnode bugfix --- code/perception/src/vision_node.py | 56 +++++++++++++++++++----------- 1 file changed, 36 insertions(+), 20 deletions(-) diff --git a/code/perception/src/vision_node.py b/code/perception/src/vision_node.py index 7345d0c0..b197b0f5 100755 --- a/code/perception/src/vision_node.py +++ b/code/perception/src/vision_node.py @@ -355,38 +355,54 @@ def predict_ultralytics(self, image): # create 2d glass plane at object # with box dimension - scale_width = abs(obj_dist1[1] - obj_dist2[1])\ - / abs(y1-y2) - scale_height = abs(obj_dist1[2] - obj_dist2[2])\ - / abs(x1-x2) - width = distances_copy.shape[1] * scale_width - height = distances_copy.shape[0] * scale_height - - # upper left - ul_x = obj_dist1[0] - ul_y = obj_dist1[1] - (-y1 + scale_width) - ul_z = obj_dist1[2] - (-x1 + scale_height) - - # lower right - lr_x = obj_dist1[0] - lr_y = ul_y + width - lr_z = ul_z + height + width_diff = abs(y1-y2) + height_diff = abs(x1-x2) + + if width_diff > 0 and height_diff > 0: + scale_width = abs(obj_dist1[1] - obj_dist2[1])\ + / width_diff + scale_height = abs(obj_dist1[2] - obj_dist2[2])\ + / height_diff + width = distances_copy.shape[1] * scale_width + height = distances_copy.shape[0] * scale_height + + # upper left + ul_x = obj_dist1[0] + ul_y = obj_dist1[1] - (-y1 + scale_width) + ul_z = obj_dist1[2] - (-x1 + scale_height) + + # lower right + lr_x = obj_dist1[0] + lr_y = ul_y + width + lr_z = ul_z + height + + distance_output.append([cls, + abs_distance, + ul_x, ul_y, ul_z, + lr_x, lr_y, lr_z]) + else: + distance_output.append([cls, + abs_distance, + np.inf, np.inf, np.inf, + np.inf, np.inf, np.inf]) else: obj_dist1 = (np.inf, np.inf, np.inf) abs_distance = np.inf + distance_output.append([cls, + abs_distance, + np.inf, np.inf, np.inf, + np.inf, np.inf, np.inf]) + c_boxes.append(torch.tensor(pixels)) c_labels.append(f"Class: {cls}," f"Meters: {round(abs_distance, 2)}," f"({round(float(obj_dist1[0]), 2)}," f"{round(float(obj_dist1[1]), 2)}," f"{round(float(obj_dist1[2]), 2)})") - distance_output.append([cls, - abs_distance, - ul_x, ul_y, ul_z, - lr_x, lr_y, lr_z]) + """print("DISTANCE_ARRAY: ", distance_output)""" self.distance_publisher.publish( Float32MultiArray(data=distance_output)) From 10964db01e72438ca5797402a96c41c8187457bc Mon Sep 17 00:00:00 2001 From: Leon Okrusch Date: Thu, 7 Mar 2024 14:14:21 +0100 Subject: [PATCH 21/33] feat(192): memory bug fix --- code/agent/config/rviz_config.rviz | 17 --------- code/agent/launch/agent.launch | 2 +- code/perception/src/vision_node.py | 57 ++++++++---------------------- 3 files changed, 15 insertions(+), 61 deletions(-) diff --git a/code/agent/config/rviz_config.rviz b/code/agent/config/rviz_config.rviz index 2e419cc2..3f32f23c 100644 --- a/code/agent/config/rviz_config.rviz +++ b/code/agent/config/rviz_config.rviz @@ -69,23 +69,6 @@ Visualization Manager: PointCloud2: false Value: true Zoom Factor: 1 - - Class: rviz/Image - Enabled: true - Image Rendering: background and overlay - Image Topic: /paf/hero/Center/segmented_image - Name: VisonNode Output - Overlay Alpha: 0.5 - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: true - Visibility: - Grid: true - Imu: true - Path: true - PointCloud2: true - Value: true - Zoom Factor: 1 - Alpha: 1 Class: rviz_plugin_tutorials/Imu Color: 204; 51; 204 diff --git a/code/agent/launch/agent.launch b/code/agent/launch/agent.launch index 089c378a..daa312e5 100644 --- a/code/agent/launch/agent.launch +++ b/code/agent/launch/agent.launch @@ -21,5 +21,5 @@ - + diff --git a/code/perception/src/vision_node.py b/code/perception/src/vision_node.py index b197b0f5..d67ba710 100755 --- a/code/perception/src/vision_node.py +++ b/code/perception/src/vision_node.py @@ -214,7 +214,7 @@ def handle_camera_image(self, image): vision_result = self.predict_ultralytics(image) # publish image to rviz - img_msg = self.bridge.cv2_to_imgmsg(vision_result, + """img_msg = self.bridge.cv2_to_imgmsg(vision_result, encoding="rgb8") img_msg.header = image.header side = rospy.resolve_name(img_msg.header.frame_id).split('/')[2] @@ -225,8 +225,9 @@ def handle_camera_image(self, image): if side == "Left": self.publisher_left.publish(img_msg) if side == "Right": - self.publisher_right.publish(img_msg) + self.publisher_right.publish(img_msg)""" + locals().clear() # print(f"Published Image on Side: {side}") pass @@ -254,6 +255,7 @@ def handle_dist_array(self, dist_array): desired_encoding='passthrough') # print("RECEIVED DIST") self.dist_arrays = dist_array + locals().clear() def predict_torch(self, image): self.model.eval() @@ -285,40 +287,6 @@ def predict_ultralytics(self, image): output = self.model(cv_image, half=True, verbose=False) - """distance_output = [] - c_boxes = [] - c_labels = [] - for r in output: - boxes = r.boxes - for box in boxes: - cls = box.cls.item() - pixels = box.xyxy[0] - print(pixels) - if len(self.depth_images) > 0: - distances = np.asarray( - [self.depth_images[i][int(pixels[1]):int(pixels[3]):1, - int(pixels[0]):int(pixels[2]):1] - for i in range(len(self.depth_images))]) - non_zero_filter = distances[distances != 0] - - if len(non_zero_filter) > 0: - obj_dist = np.min(non_zero_filter) - else: - obj_dist = np.inf - - c_boxes.append(torch.tensor(pixels)) - c_labels.append(f"Class: {cls}, Meters: {obj_dist}") - distance_output.append([cls, obj_dist]) - - # print(distance_output) - # self.logerr(distance_output) - self.distance_publisher.publish( - Float32MultiArray(data=distance_output)) - - transposed_image = np.transpose(cv_image, (2, 0, 1)) - image_np_with_detections = torch.tensor(transposed_image, - dtype=torch.uint8)""" - # handle distance of objects distance_output = [] c_boxes = [] @@ -405,16 +373,15 @@ def predict_ultralytics(self, image): """print("DISTANCE_ARRAY: ", distance_output)""" self.distance_publisher.publish( Float32MultiArray(data=distance_output)) - - transposed_image = np.transpose(cv_image, (2, 0, 1)) + + """transposed_image = np.transpose(cv_image, (2, 0, 1)) image_np_with_detections = torch.tensor(transposed_image, - dtype=torch.uint8) + dtype=torch.uint8)""" if 9 in output[0].boxes.cls: self.process_traffic_lights(output[0], cv_image, image.header) - c_boxes = torch.stack(c_boxes) - # print(image_np_with_detections.shape, c_boxes.shape, c_labels) + """c_boxes = torch.stack(c_boxes) box = draw_bounding_boxes(image_np_with_detections, c_boxes, c_labels, @@ -423,8 +390,10 @@ def predict_ultralytics(self, image): font_size=12) np_box_img = np.transpose(box.detach().numpy(), (1, 2, 0)) - box_img = cv2.cvtColor(np_box_img, cv2.COLOR_BGR2RGB) - return box_img + box_img = cv2.cvtColor(np_box_img, cv2.COLOR_BGR2RGB)""" + locals().clear() + # return box_img + # return output[0].plot() def process_traffic_lights(self, prediction, cv_image, image_header): @@ -453,6 +422,8 @@ def process_traffic_lights(self, prediction, cv_image, image_header): encoding="rgb8") traffic_light_image.header = image_header self.traffic_light_publisher.publish(traffic_light_image) + + locals().clear() def create_mask(self, input_image, model_output): output_predictions = torch.argmax(model_output, dim=0) From a36c7ab96b5a359259aeca9aac9bfe43b8c64803 Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Thu, 7 Mar 2024 14:15:22 +0100 Subject: [PATCH 22/33] feat: Vision Node filter --- .../src/local_planner/collision_check.py | 27 +++++++++++++++++-- code/planning/src/local_planner/test_utils.py | 25 +++++++++++++++++ code/planning/src/local_planner/utils.py | 24 +++++++++++++++++ 3 files changed, 74 insertions(+), 2 deletions(-) create mode 100644 code/planning/src/local_planner/test_utils.py diff --git a/code/planning/src/local_planner/collision_check.py b/code/planning/src/local_planner/collision_check.py index ab1047a7..bc349790 100755 --- a/code/planning/src/local_planner/collision_check.py +++ b/code/planning/src/local_planner/collision_check.py @@ -6,6 +6,7 @@ import ros_compatibility as roscomp from ros_compatibility.node import CompatibleNode from rospy import Subscriber +from rospy.numpy_msg import numpy_msg from carla_msgs.msg import CarlaSpeedometer # , CarlaWorldInfo # from std_msgs.msg import String from std_msgs.msg import Float32, Float32MultiArray @@ -34,8 +35,8 @@ def __init__(self): # Subscriber for lidar distance # TODO: Change to real lidar distance self.lidar_dist = self.new_subscription( - Float32, - f"/carla/{self.role_name}/LIDAR_range", + numpy_msg(Float32MultiArray), + f"/carla/{self.role_name}/Center/object_distance", self.__set_distance, qos_profile=1) # Publisher for emergency stop @@ -59,6 +60,28 @@ def __init__(self): self.__object_last_position: tuple = None self.logdebug("CollisionCheck started") + def filter_vision_objects(self, data): + """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 + of the bounding box + + Array shape: [classID, EuclidDistance, + UpperLeft(x,y,z), LowerRight(x,y,z)] + + Args: + data (FloatMultiArray): numpy array with vision objects + """ + float_array = data.data + # Filter out all objects that are not cars + all_cars = float_array[np.where(float_array[:, 0] == 2)] + # Filter out parking cars or cars on opposite lane + no_oncoming_traffic = all_cars[np.where(all_cars[:, 6] < 0.5)] + no_parking_cars = no_oncoming_traffic[ + np.where(no_oncoming_traffic[:, 6] > -3)] + # Return nearest car + return no_parking_cars[np.argmin(no_parking_cars[:, 1])] + def update_distance(self): """Updates the distance to the obstacle in front """ diff --git a/code/planning/src/local_planner/test_utils.py b/code/planning/src/local_planner/test_utils.py new file mode 100644 index 00000000..76b2be85 --- /dev/null +++ b/code/planning/src/local_planner/test_utils.py @@ -0,0 +1,25 @@ +import unittest +import numpy as np +from utils import filter_vision_objects + + +class TestUtils(unittest.TestCase): + def test_filter_vision_objects(self): + # Create a sample input data + data = np.array([[1, 2.5, 1.0, 2.0, 3.0, 4.0, 0.2], + [2, 3.5, 2.0, 3.0, 4.0, 5.0, -1.0], + [2, 4.5, 3.0, 4.0, 5.0, 6.0, -2.0], + [3, 5.5, 4.0, 5.0, 6.0, 7.0, 0.8]]) + + # Call the method under test + result = filter_vision_objects(data) + + # Define the expected output + expected_output = np.array([2, 3.5, 2.0, 3.0, 4.0, 5.0, -1.0]) + + # Assert that the result matches the expected output + np.testing.assert_array_equal(result, expected_output) + + +if __name__ == '__main__': + unittest.main() \ No newline at end of file diff --git a/code/planning/src/local_planner/utils.py b/code/planning/src/local_planner/utils.py index 6a696588..89ffe049 100644 --- a/code/planning/src/local_planner/utils.py +++ b/code/planning/src/local_planner/utils.py @@ -2,6 +2,7 @@ import numpy as np import math + hyperparameters = { "max_speed": 15, "max_accel": 4.0, @@ -153,3 +154,26 @@ def spawn_car(distance): spectator.set_transform( carla.Transform(ego_vehicle.get_location() + carla.Location(z=50), carla.Rotation(pitch=-90))) + + +def filter_vision_objects(float_array): + """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 + of the bounding box + + Array shape: [classID, EuclidDistance, + UpperLeft(x,y,z), LowerRight(x,y,z)] + + Args: + data (FloatMultiArray): numpy array with vision objects + """ + # float_array = data.data + # Filter out all objects that are not cars + all_cars = float_array[np.where(float_array[:, 0] == 2)] + # Filter out parking cars or cars on opposite lane + no_oncoming_traffic = all_cars[np.where(all_cars[:, 6] < 0.5)] + no_parking_cars = no_oncoming_traffic[ + np.where(no_oncoming_traffic[:, 6] > -3)] + # Return nearest car + return no_parking_cars[np.argmin(no_parking_cars[:, 1])] From bc4ba14b3c255a30a70f24e39e69b3ed7ed8afd2 Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Thu, 7 Mar 2024 14:57:45 +0100 Subject: [PATCH 23/33] feat: Incooperate object from Vision Node in collision check --- code/perception/src/vision_node.py | 6 +-- .../src/local_planner/collision_check.py | 46 ++++++------------- code/planning/src/local_planner/test_utils.py | 25 ---------- code/planning/src/local_planner/utils.py | 14 +++++- 4 files changed, 30 insertions(+), 61 deletions(-) delete mode 100644 code/planning/src/local_planner/test_utils.py diff --git a/code/perception/src/vision_node.py b/code/perception/src/vision_node.py index d67ba710..476f13ca 100755 --- a/code/perception/src/vision_node.py +++ b/code/perception/src/vision_node.py @@ -19,7 +19,7 @@ from torchvision.utils import draw_bounding_boxes, draw_segmentation_masks import numpy as np from ultralytics import NAS, YOLO, RTDETR, SAM, FastSAM -import rospy +# import rospy """ VisionNode: @@ -373,7 +373,7 @@ def predict_ultralytics(self, image): """print("DISTANCE_ARRAY: ", distance_output)""" self.distance_publisher.publish( Float32MultiArray(data=distance_output)) - + """transposed_image = np.transpose(cv_image, (2, 0, 1)) image_np_with_detections = torch.tensor(transposed_image, dtype=torch.uint8)""" @@ -422,7 +422,7 @@ def process_traffic_lights(self, prediction, cv_image, image_header): encoding="rgb8") traffic_light_image.header = image_header self.traffic_light_publisher.publish(traffic_light_image) - + locals().clear() def create_mask(self, input_image, model_output): diff --git a/code/planning/src/local_planner/collision_check.py b/code/planning/src/local_planner/collision_check.py index bc349790..cdeb6d47 100755 --- a/code/planning/src/local_planner/collision_check.py +++ b/code/planning/src/local_planner/collision_check.py @@ -11,6 +11,7 @@ # from std_msgs.msg import String from std_msgs.msg import Float32, Float32MultiArray from std_msgs.msg import Bool +from utils import filter_vision_objects class CollisionCheck(CompatibleNode): @@ -60,32 +61,11 @@ def __init__(self): self.__object_last_position: tuple = None self.logdebug("CollisionCheck started") - def filter_vision_objects(self, data): - """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 - of the bounding box - - Array shape: [classID, EuclidDistance, - UpperLeft(x,y,z), LowerRight(x,y,z)] - - Args: - data (FloatMultiArray): numpy array with vision objects - """ - float_array = data.data - # Filter out all objects that are not cars - all_cars = float_array[np.where(float_array[:, 0] == 2)] - # Filter out parking cars or cars on opposite lane - no_oncoming_traffic = all_cars[np.where(all_cars[:, 6] < 0.5)] - no_parking_cars = no_oncoming_traffic[ - np.where(no_oncoming_traffic[:, 6] > -3)] - # Return nearest car - return no_parking_cars[np.argmin(no_parking_cars[:, 1])] - - def update_distance(self): + def update_distance(self, reset): """Updates the distance to the obstacle in front """ - if np.isinf(self.__object_last_position[1]): + if reset: + # Reset all values if we do not have car in front self.__object_last_position = None self.__object_first_position = None return @@ -94,20 +74,24 @@ def update_distance(self): self.__object_last_position = None return - def __set_distance(self, data: Float32): + def __set_distance(self, data: Float32MultiArray): """Saves last distance from LIDAR Args: data (Float32): Message from lidar with distance """ - 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): + nearest_object = filter_vision_objects(data.data) + if nearest_object is None: + self.update_distance(True) return + # 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(), data.data) - self.update_distance() + self.__object_last_position = (rospy.get_rostime(), nearest_object[1]) + self.update_distance(False) self.calculate_obstacle_speed() def calculate_obstacle_speed(self): diff --git a/code/planning/src/local_planner/test_utils.py b/code/planning/src/local_planner/test_utils.py deleted file mode 100644 index 76b2be85..00000000 --- a/code/planning/src/local_planner/test_utils.py +++ /dev/null @@ -1,25 +0,0 @@ -import unittest -import numpy as np -from utils import filter_vision_objects - - -class TestUtils(unittest.TestCase): - def test_filter_vision_objects(self): - # Create a sample input data - data = np.array([[1, 2.5, 1.0, 2.0, 3.0, 4.0, 0.2], - [2, 3.5, 2.0, 3.0, 4.0, 5.0, -1.0], - [2, 4.5, 3.0, 4.0, 5.0, 6.0, -2.0], - [3, 5.5, 4.0, 5.0, 6.0, 7.0, 0.8]]) - - # Call the method under test - result = filter_vision_objects(data) - - # Define the expected output - expected_output = np.array([2, 3.5, 2.0, 3.0, 4.0, 5.0, -1.0]) - - # Assert that the result matches the expected output - np.testing.assert_array_equal(result, expected_output) - - -if __name__ == '__main__': - unittest.main() \ No newline at end of file diff --git a/code/planning/src/local_planner/utils.py b/code/planning/src/local_planner/utils.py index 89ffe049..7eccf6c0 100644 --- a/code/planning/src/local_planner/utils.py +++ b/code/planning/src/local_planner/utils.py @@ -166,14 +166,24 @@ def filter_vision_objects(float_array): UpperLeft(x,y,z), LowerRight(x,y,z)] Args: - data (FloatMultiArray): numpy array with vision objects + data (ndarray): numpy array with vision objects """ - # float_array = data.data + # Filter all rows that contain np.inf + float_array = float_array[~np.any(np.isinf(float_array), axis=1), :] + if float_array.size == 0: + return None # Filter out all objects that are not cars all_cars = float_array[np.where(float_array[:, 0] == 2)] # Filter out parking cars or cars on opposite lane no_oncoming_traffic = all_cars[np.where(all_cars[:, 6] < 0.5)] + + if no_oncoming_traffic.size == 0: + return None + no_parking_cars = no_oncoming_traffic[ np.where(no_oncoming_traffic[:, 6] > -3)] + + if no_parking_cars.size == 0: + return None # Return nearest car return no_parking_cars[np.argmin(no_parking_cars[:, 1])] From e5251a4566a2f3348ee7f362ac173913a61bfc9a Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Thu, 7 Mar 2024 16:08:06 +0100 Subject: [PATCH 24/33] fix: Vision Node error --- build/docker-compose.yml | 2 +- code/agent/launch/agent.launch | 2 +- .../src/global_plan_distance_publisher.py | 2 +- code/perception/src/vision_node.py | 41 ++++++++++++------- code/planning/launch/planning.launch | 4 +- .../src/global_planner/global_planner.py | 4 +- .../src/local_planner/collision_check.py | 6 +-- 7 files changed, 36 insertions(+), 25 deletions(-) diff --git a/build/docker-compose.yml b/build/docker-compose.yml index 6ba17e3d..7b3cf6f3 100644 --- a/build/docker-compose.yml +++ b/build/docker-compose.yml @@ -63,7 +63,7 @@ services: 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=paf23-carla-simulator-1 --track=MAP" + #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=paf23-carla-simulator-1 --track=MAP" logging: driver: "local" diff --git a/code/agent/launch/agent.launch b/code/agent/launch/agent.launch index daa312e5..089c378a 100644 --- a/code/agent/launch/agent.launch +++ b/code/agent/launch/agent.launch @@ -21,5 +21,5 @@ - + diff --git a/code/perception/src/global_plan_distance_publisher.py b/code/perception/src/global_plan_distance_publisher.py index 4e4cf37b..40da367c 100755 --- a/code/perception/src/global_plan_distance_publisher.py +++ b/code/perception/src/global_plan_distance_publisher.py @@ -47,7 +47,7 @@ def __init__(self): # Change comment for dev_launch self.global_plan_subscriber = self.new_subscription( CarlaRoute, - "/paf/" + self.role_name + "/global_plan", + "/carla/" + self.role_name + "/global_plan", self.update_global_route, qos_profile=1) # self.global_plan_subscriber = self.new_subscription( diff --git a/code/perception/src/vision_node.py b/code/perception/src/vision_node.py index 476f13ca..e93b9e71 100755 --- a/code/perception/src/vision_node.py +++ b/code/perception/src/vision_node.py @@ -191,7 +191,7 @@ def setup_camera_publishers(self): def setup_object_distance_publishers(self): self.distance_publisher = self.new_publisher( - msg_type=numpy_msg(Float32MultiArray), + msg_type=Float32MultiArray, topic=f"/paf/{self.role_name}/{self.side}/object_distance", qos_profile=1) @@ -344,24 +344,35 @@ def predict_ultralytics(self, image): lr_y = ul_y + width lr_z = ul_z + height - distance_output.append([cls, - abs_distance, - ul_x, ul_y, ul_z, - lr_x, lr_y, lr_z]) - else: - distance_output.append([cls, - abs_distance, - np.inf, np.inf, np.inf, - np.inf, np.inf, np.inf]) + distance_output.append(float(cls)) + distance_output.append(float(abs_distance)) + distance_output.append(float(ul_x)) + distance_output.append(float(ul_y)) + distance_output.append(float(ul_z)) + distance_output.append(float(lr_x)) + distance_output.append(float(lr_y)) + distance_output.append(float(lr_z)) + else: + distance_output.append(float(cls)) + distance_output.append(float(abs_distance)) + distance_output.append(float(np.inf)) + distance_output.append(float(np.inf)) + distance_output.append(float(np.inf)) + distance_output.append(float(np.inf)) + distance_output.append(float(np.inf)) + distance_output.append(float(np.inf)) else: obj_dist1 = (np.inf, np.inf, np.inf) abs_distance = np.inf - - distance_output.append([cls, - abs_distance, - np.inf, np.inf, np.inf, - np.inf, np.inf, np.inf]) + distance_output.append(float(cls)) + distance_output.append(float(abs_distance)) + distance_output.append(float(np.inf)) + distance_output.append(float(np.inf)) + distance_output.append(float(np.inf)) + distance_output.append(float(np.inf)) + distance_output.append(float(np.inf)) + distance_output.append(float(np.inf)) c_boxes.append(torch.tensor(pixels)) c_labels.append(f"Class: {cls}," diff --git a/code/planning/launch/planning.launch b/code/planning/launch/planning.launch index 241786f0..3f3948ba 100644 --- a/code/planning/launch/planning.launch +++ b/code/planning/launch/planning.launch @@ -16,13 +16,13 @@ - + diff --git a/code/planning/src/global_planner/global_planner.py b/code/planning/src/global_planner/global_planner.py index 8d8c2743..23442530 100755 --- a/code/planning/src/global_planner/global_planner.py +++ b/code/planning/src/global_planner/global_planner.py @@ -57,7 +57,7 @@ def __init__(self): # uncomment /paf/hero/global_plan and comment /carla/... for dev_launch self.global_plan_sub = self.new_subscription( msg_type=CarlaRoute, - topic='/paf/' + self.role_name + '/global_plan', + topic='/carla/' + self.role_name + '/global_plan', callback=self.global_route_callback, qos_profile=10) # self.global_plan_sub = self.new_subscription( @@ -84,7 +84,7 @@ def __init__(self): self.logdebug('PrePlanner-Node started') # uncomment for self.dev_load_world_info() for dev_launch - self.dev_load_world_info() + # self.dev_load_world_info() def global_route_callback(self, data: CarlaRoute) -> None: """ diff --git a/code/planning/src/local_planner/collision_check.py b/code/planning/src/local_planner/collision_check.py index cdeb6d47..a547cc2a 100755 --- a/code/planning/src/local_planner/collision_check.py +++ b/code/planning/src/local_planner/collision_check.py @@ -6,7 +6,7 @@ import ros_compatibility as roscomp from ros_compatibility.node import CompatibleNode from rospy import Subscriber -from rospy.numpy_msg import numpy_msg +# from rospy.numpy_msg import numpy_msg from carla_msgs.msg import CarlaSpeedometer # , CarlaWorldInfo # from std_msgs.msg import String from std_msgs.msg import Float32, Float32MultiArray @@ -36,8 +36,8 @@ def __init__(self): # Subscriber for lidar distance # TODO: Change to real lidar distance self.lidar_dist = self.new_subscription( - numpy_msg(Float32MultiArray), - f"/carla/{self.role_name}/Center/object_distance", + Float32MultiArray, + f"/paf/{self.role_name}/Center/object_distance", self.__set_distance, qos_profile=1) # Publisher for emergency stop From a9d87372ce88e71ef2660c9113fde14063303a48 Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Thu, 7 Mar 2024 16:34:32 +0100 Subject: [PATCH 25/33] fix: testing --- code/planning/launch/planning.launch | 4 ++-- code/planning/src/global_planner/dev_global_route.py | 2 +- code/planning/src/global_planner/global_planner.py | 2 +- code/planning/src/local_planner/collision_check.py | 2 ++ code/planning/src/local_planner/motion_planning.py | 1 - code/planning/src/local_planner/utils.py | 9 ++++++++- 6 files changed, 14 insertions(+), 6 deletions(-) diff --git a/code/planning/launch/planning.launch b/code/planning/launch/planning.launch index 3f3948ba..241786f0 100644 --- a/code/planning/launch/planning.launch +++ b/code/planning/launch/planning.launch @@ -16,13 +16,13 @@ - + diff --git a/code/planning/src/global_planner/dev_global_route.py b/code/planning/src/global_planner/dev_global_route.py index 824bafa5..1857293c 100755 --- a/code/planning/src/global_planner/dev_global_route.py +++ b/code/planning/src/global_planner/dev_global_route.py @@ -51,7 +51,7 @@ def __init__(self): self.global_plan_pub = self.new_publisher( msg_type=CarlaRoute, - topic='/paf/' + self.role_name + '/global_plan', + topic='/carla/' + self.role_name + '/global_plan', qos_profile=QoSProfile( depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL) diff --git a/code/planning/src/global_planner/global_planner.py b/code/planning/src/global_planner/global_planner.py index 23442530..be526975 100755 --- a/code/planning/src/global_planner/global_planner.py +++ b/code/planning/src/global_planner/global_planner.py @@ -84,7 +84,7 @@ def __init__(self): self.logdebug('PrePlanner-Node started') # uncomment for self.dev_load_world_info() for dev_launch - # self.dev_load_world_info() + self.dev_load_world_info() def global_route_callback(self, data: CarlaRoute) -> None: """ diff --git a/code/planning/src/local_planner/collision_check.py b/code/planning/src/local_planner/collision_check.py index a547cc2a..29915573 100755 --- a/code/planning/src/local_planner/collision_check.py +++ b/code/planning/src/local_planner/collision_check.py @@ -84,6 +84,7 @@ def __set_distance(self, data: Float32MultiArray): if nearest_object is None: self.update_distance(True) return + self.logerr("Obstacle detected: " + nearest_object) # if np.isinf(data.data) and \ # self.__object_last_position is not None and \ # rospy.get_rostime() - self.__object_last_position[0] < \ @@ -99,6 +100,7 @@ def calculate_obstacle_speed(self): based on the distance between to timestamps """ # Check if current speed from vehicle is not None + self.logerr("Obstacle detected and Speed calculated: " + self.__object_last_position[1]) if self.__current_velocity is None or \ self.__object_first_position is None or \ self.__object_last_position is None: diff --git a/code/planning/src/local_planner/motion_planning.py b/code/planning/src/local_planner/motion_planning.py index 61158bc5..9056107a 100755 --- a/code/planning/src/local_planner/motion_planning.py +++ b/code/planning/src/local_planner/motion_planning.py @@ -379,7 +379,6 @@ def update_target_speed(self, acc_speed, behavior): else: self.target_speed = be_speed # self.target_speed = min(self.target_speed, 8) - self.loginfo(f"Speed: {self.target_speed}") self.velocity_pub.publish(self.target_speed) # self.logerr(f"Speed: {self.target_speed}") # self.speed_list.append(self.target_speed) diff --git a/code/planning/src/local_planner/utils.py b/code/planning/src/local_planner/utils.py index 7eccf6c0..c8e547de 100644 --- a/code/planning/src/local_planner/utils.py +++ b/code/planning/src/local_planner/utils.py @@ -1,6 +1,7 @@ from scipy.spatial.transform import Rotation import numpy as np import math +import rospy hyperparameters = { @@ -168,15 +169,21 @@ def filter_vision_objects(float_array): Args: data (ndarray): numpy array with vision objects """ + # Reshape array to 8 columns and n rows (one row per object) + float_array = np.asarray(float_array) + float_array = np.reshape(float_array, (float_array.size//8, 8)) # Filter all rows that contain np.inf + rospy.logerr("Before filtering inf: " + str(float_array)) float_array = float_array[~np.any(np.isinf(float_array), axis=1), :] + rospy.logerr("After filtering inf: " + str(float_array)) if float_array.size == 0: return None # Filter out all objects that are not cars all_cars = float_array[np.where(float_array[:, 0] == 2)] + # Filter out parking cars or cars on opposite lane no_oncoming_traffic = all_cars[np.where(all_cars[:, 6] < 0.5)] - + rospy.logerr("After filtering left lane: " + str(no_oncoming_traffic)) if no_oncoming_traffic.size == 0: return None From c6fce8ebbe10bf5d0cc959e66eafb434fde2d4ac Mon Sep 17 00:00:00 2001 From: Leon Okrusch Date: Thu, 7 Mar 2024 17:32:16 +0100 Subject: [PATCH 26/33] feat(192): reconfiguration of 3d points and publisher --- code/perception/src/vision_node.py | 68 ++++++++++--------- .../src/local_planner/collision_check.py | 3 +- code/planning/src/local_planner/utils.py | 17 ++++- 3 files changed, 53 insertions(+), 35 deletions(-) diff --git a/code/perception/src/vision_node.py b/code/perception/src/vision_node.py index e93b9e71..afebd67c 100755 --- a/code/perception/src/vision_node.py +++ b/code/perception/src/vision_node.py @@ -307,26 +307,42 @@ def predict_ultralytics(self, image): distances_copy[distances_copy == 0] = np.inf if len(non_zero_filter) > 0: - sorted_indices = np.argsort(distances_copy[:, :, 0], - axis=None) - x1, y1 = np.unravel_index(sorted_indices[0], - distances_copy.shape[:2]) - x2, y2 = np.unravel_index(sorted_indices[1], + min_x_sorted_indices = np.argsort( + distances_copy[:, :, 0], + axis=None) + x1, y1 = np.unravel_index(min_x_sorted_indices[0], distances_copy.shape[:2]) + """x2, y2 = np.unravel_index(min_x_sorted_indices[1], + distances_copy.shape[:2])""" + + abs_distance_copy = np.abs(distances_copy.copy()) + min_y_sorted_indices = np.argsort( + abs_distance_copy[:, :, 1], + axis=None) + x3, y3 = np.unravel_index(min_y_sorted_indices[0], + abs_distance_copy.shape[:2]) + """ + x4, y4 = np.unravel_index(min_y_sorted_indices[1], + abs_distance_copy.shape[:2]) + """ + obj_dist1 = distances_copy[x1][y1].copy() - obj_dist2 = distances_copy[x2][y2].copy() + # obj_dist2 = distances_copy[x2][y2].copy() + obj_dist3 = distances_copy[x3][y3].copy() + # obj_dist4 = distances_copy[x4][y4].copy() + # print(obj_dist1, obj_dist3) - abs_distance = np.sqrt( + """abs_distance = np.sqrt( obj_dist1[0]**2 + obj_dist1[1]**2 + - obj_dist1[2]**2) + obj_dist1[2]**2)""" # create 2d glass plane at object # with box dimension - width_diff = abs(y1-y2) - height_diff = abs(x1-x2) + # width_diff = abs(y1-y2) + # height_diff = abs(x1-x2) - if width_diff > 0 and height_diff > 0: + """if width_diff > 0 and height_diff > 0: scale_width = abs(obj_dist1[1] - obj_dist2[1])\ / width_diff scale_height = abs(obj_dist1[2] - obj_dist2[2])\ @@ -344,35 +360,23 @@ def predict_ultralytics(self, image): lr_y = ul_y + width lr_z = ul_z + height - distance_output.append(float(cls)) - distance_output.append(float(abs_distance)) - distance_output.append(float(ul_x)) - distance_output.append(float(ul_y)) - distance_output.append(float(ul_z)) - distance_output.append(float(lr_x)) - distance_output.append(float(lr_y)) - distance_output.append(float(lr_z)) - - else: - distance_output.append(float(cls)) - distance_output.append(float(abs_distance)) - distance_output.append(float(np.inf)) - distance_output.append(float(np.inf)) - distance_output.append(float(np.inf)) - distance_output.append(float(np.inf)) - distance_output.append(float(np.inf)) - distance_output.append(float(np.inf)) + # -5 < y < 8""" + + distance_output.append(float(cls)) + distance_output.append(float(obj_dist1[0])) + distance_output.append(float(obj_dist3[1])) + else: obj_dist1 = (np.inf, np.inf, np.inf) abs_distance = np.inf - distance_output.append(float(cls)) + """distance_output.append(float(cls)) distance_output.append(float(abs_distance)) distance_output.append(float(np.inf)) distance_output.append(float(np.inf)) distance_output.append(float(np.inf)) distance_output.append(float(np.inf)) distance_output.append(float(np.inf)) - distance_output.append(float(np.inf)) + distance_output.append(float(np.inf))""" c_boxes.append(torch.tensor(pixels)) c_labels.append(f"Class: {cls}," @@ -381,7 +385,7 @@ def predict_ultralytics(self, image): f"{round(float(obj_dist1[1]), 2)}," f"{round(float(obj_dist1[2]), 2)})") - """print("DISTANCE_ARRAY: ", distance_output)""" + print("DISTANCE_ARRAY: ", distance_output) self.distance_publisher.publish( Float32MultiArray(data=distance_output)) diff --git a/code/planning/src/local_planner/collision_check.py b/code/planning/src/local_planner/collision_check.py index 29915573..9f7bb23d 100755 --- a/code/planning/src/local_planner/collision_check.py +++ b/code/planning/src/local_planner/collision_check.py @@ -100,7 +100,8 @@ def calculate_obstacle_speed(self): based on the distance between to timestamps """ # Check if current speed from vehicle is not None - self.logerr("Obstacle detected and Speed calculated: " + self.__object_last_position[1]) + self.logerr("Obstacle detected and Speed calculated: " + + self.__object_last_position[1]) if self.__current_velocity is None or \ self.__object_first_position is None or \ self.__object_last_position is None: diff --git a/code/planning/src/local_planner/utils.py b/code/planning/src/local_planner/utils.py index c8e547de..5e7ad5f5 100644 --- a/code/planning/src/local_planner/utils.py +++ b/code/planning/src/local_planner/utils.py @@ -169,9 +169,22 @@ def filter_vision_objects(float_array): Args: data (ndarray): numpy array with vision objects """ + + """ + LEON: + + Ihr bekommt jetzt nur 3-Werte -> ClassIndex, Min_X, Min_Abs_Y + + Min_Abs_Y ist der nähste Punkt vom Object zu Y=0 also der Mitte. + + Damit habt ihr immer automatisch den + nähesten und wichtigsten Punkt des Objekts. + + """ + # Reshape array to 8 columns and n rows (one row per object) float_array = np.asarray(float_array) - float_array = np.reshape(float_array, (float_array.size//8, 8)) + float_array = np.reshape(float_array, (float_array.size//3, 3)) # Filter all rows that contain np.inf rospy.logerr("Before filtering inf: " + str(float_array)) float_array = float_array[~np.any(np.isinf(float_array), axis=1), :] @@ -180,7 +193,7 @@ def filter_vision_objects(float_array): return None # Filter out all objects that are not cars all_cars = float_array[np.where(float_array[:, 0] == 2)] - + # Filter out parking cars or cars on opposite lane no_oncoming_traffic = all_cars[np.where(all_cars[:, 6] < 0.5)] rospy.logerr("After filtering left lane: " + str(no_oncoming_traffic)) From fbc4a7cf091851f172aa3de04724eddb0a359ba3 Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Fri, 8 Mar 2024 13:48:57 +0100 Subject: [PATCH 27/33] fix: linter --- code/planning/src/local_planner/collision_check.py | 3 ++- code/planning/src/local_planner/utils.py | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/code/planning/src/local_planner/collision_check.py b/code/planning/src/local_planner/collision_check.py index 29915573..9f7bb23d 100755 --- a/code/planning/src/local_planner/collision_check.py +++ b/code/planning/src/local_planner/collision_check.py @@ -100,7 +100,8 @@ def calculate_obstacle_speed(self): based on the distance between to timestamps """ # Check if current speed from vehicle is not None - self.logerr("Obstacle detected and Speed calculated: " + self.__object_last_position[1]) + self.logerr("Obstacle detected and Speed calculated: " + + self.__object_last_position[1]) if self.__current_velocity is None or \ self.__object_first_position is None or \ self.__object_last_position is None: diff --git a/code/planning/src/local_planner/utils.py b/code/planning/src/local_planner/utils.py index c8e547de..0f00120e 100644 --- a/code/planning/src/local_planner/utils.py +++ b/code/planning/src/local_planner/utils.py @@ -180,7 +180,7 @@ def filter_vision_objects(float_array): return None # Filter out all objects that are not cars all_cars = float_array[np.where(float_array[:, 0] == 2)] - + # Filter out parking cars or cars on opposite lane no_oncoming_traffic = all_cars[np.where(all_cars[:, 6] < 0.5)] rospy.logerr("After filtering left lane: " + str(no_oncoming_traffic)) From 6ce2855dbcf605167a55fcbfb3ae39cd24966693 Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Fri, 8 Mar 2024 16:35:06 +0100 Subject: [PATCH 28/33] feat: Adjusting to perception output --- code/perception/src/vision_node.py | 12 +-- .../src/local_planner/collision_check.py | 12 ++- .../src/local_planner/motion_planning.py | 82 ++++++++++++------- code/planning/src/local_planner/utils.py | 29 +++---- 4 files changed, 79 insertions(+), 56 deletions(-) diff --git a/code/perception/src/vision_node.py b/code/perception/src/vision_node.py index afebd67c..519a968f 100755 --- a/code/perception/src/vision_node.py +++ b/code/perception/src/vision_node.py @@ -379,13 +379,13 @@ def predict_ultralytics(self, image): distance_output.append(float(np.inf))""" c_boxes.append(torch.tensor(pixels)) - c_labels.append(f"Class: {cls}," - f"Meters: {round(abs_distance, 2)}," - f"({round(float(obj_dist1[0]), 2)}," - f"{round(float(obj_dist1[1]), 2)}," - f"{round(float(obj_dist1[2]), 2)})") + # c_labels.append(f"Class: {cls}," + # f"Meters: {round(abs_distance, 2)}," + # f"({round(float(obj_dist1[0]), 2)}," + # f"{round(float(obj_dist1[1]), 2)}," + # f"{round(float(obj_dist1[2]), 2)})") - print("DISTANCE_ARRAY: ", distance_output) + # print("DISTANCE_ARRAY: ", distance_output) self.distance_publisher.publish( Float32MultiArray(data=distance_output)) diff --git a/code/planning/src/local_planner/collision_check.py b/code/planning/src/local_planner/collision_check.py index 9f7bb23d..da5fef8f 100755 --- a/code/planning/src/local_planner/collision_check.py +++ b/code/planning/src/local_planner/collision_check.py @@ -81,10 +81,15 @@ def __set_distance(self, data: Float32MultiArray): data (Float32): Message from lidar with distance """ nearest_object = filter_vision_objects(data.data) - if nearest_object is None: + 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): self.update_distance(True) return - self.logerr("Obstacle detected: " + nearest_object) + 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] < \ @@ -100,8 +105,6 @@ def calculate_obstacle_speed(self): based on the distance between to timestamps """ # Check if current speed from vehicle is not None - self.logerr("Obstacle detected and Speed calculated: " - + self.__object_last_position[1]) if self.__current_velocity is None or \ self.__object_first_position is None or \ self.__object_last_position is None: @@ -207,6 +210,7 @@ def check_crash(self, obstacle): self.collision_pub.publish(data) else: # If no collision is ahead publish np.Inf + self.logerr("No collision") data = Float32MultiArray(data=[np.Inf, obstacle_speed]) self.collision_pub.publish(data) diff --git a/code/planning/src/local_planner/motion_planning.py b/code/planning/src/local_planner/motion_planning.py index 9056107a..fc6baed7 100755 --- a/code/planning/src/local_planner/motion_planning.py +++ b/code/planning/src/local_planner/motion_planning.py @@ -19,7 +19,7 @@ from behavior_agent.behaviours import behavior_speed as bs from utils import convert_to_ms, approx_obstacle_pos, \ - hyperparameters, spawn_car + hyperparameters, spawn_car, location_to_gps # from scipy.spatial._kdtree import KDTree @@ -54,6 +54,7 @@ def __init__(self): self.speed_limit = None self.__corners = None self.__in_corner = False + self.calculated = False # Subscriber self.test_sub = self.new_subscription( Float32, @@ -184,41 +185,57 @@ 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): - limit_waypoints = 30 + """update trajectory for overtaking and convert it + to a new Path message + + Args: + distance_obj (float): distance to overtake object + """ pose_list = self.trajectory.poses count_retrys = 0 - def calculate_overtake(distance): - obstacle_position = approx_obstacle_pos(distance, - self.current_heading, - self.current_pos, - self.current_speed) - # trajectory_np = self.convert_pose_to_array(pose_list) - # wp=KDTree(trajectory_np[:,:2]).query(obstacle_position[0][:2])[1] - selection = pose_list[int(self.current_wp):int(self.current_wp) + - int(distance + limit_waypoints)] - waypoints = self.convert_pose_to_array(selection) - - initial_conditions = { - 'ps': 0, - '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) - 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 = calculate_overtake(distance_obj) + costs, success = self.calculate_overtake(distance_obj, + pose_list) self.overtake_success_pub.publish(float(success)) success_overtake = success count_retrys += 1 @@ -344,7 +361,8 @@ def map_corner(dist): else: return self.__get_speed_cruise() - def convert_pose_to_array(self, poses: np.array): + @staticmethod + def convert_pose_to_array(poses: np.array): """convert pose array to numpy array Args: @@ -401,8 +419,12 @@ def __set_change_point(self, data: LaneChange): (data.distance, data.isLaneChange, data.roadOption) def __set_collision_point(self, data: Float32MultiArray): - if data is not None: + if data.data is not None: self.__collision_point = data.data[0] + if self.__collision_point != np.inf and not self.calculated: + self.change_trajectory(self.__collision_point) + self.traj_pub.publish(self.trajectory) + self.calculated = True def get_speed_by_behavior(self, behavior: str) -> float: speed = 0.0 diff --git a/code/planning/src/local_planner/utils.py b/code/planning/src/local_planner/utils.py index 5e7ad5f5..bd07fe5a 100644 --- a/code/planning/src/local_planner/utils.py +++ b/code/planning/src/local_planner/utils.py @@ -1,14 +1,14 @@ from scipy.spatial.transform import Rotation import numpy as np import math -import rospy +# import rospy hyperparameters = { "max_speed": 15, "max_accel": 4.0, "max_curvature": 30.0, - "max_road_width_l": 0.1, + "max_road_width_l": 4, "max_road_width_r": 4, "d_road_w": 0.2, "dt": 0.2, @@ -25,7 +25,7 @@ "ko": 0.1, "klat": 1.0, "klon": 1.0, - "num_threads": 3, # set 0 to avoid using threaded algorithm + "num_threads": 1, # set 0 to avoid using threaded algorithm } @@ -182,28 +182,25 @@ def filter_vision_objects(float_array): """ - # Reshape array to 8 columns and n rows (one row per object) + # Reshape array to 3 columns and n rows (one row per object) float_array = np.asarray(float_array) float_array = np.reshape(float_array, (float_array.size//3, 3)) # Filter all rows that contain np.inf - rospy.logerr("Before filtering inf: " + str(float_array)) float_array = float_array[~np.any(np.isinf(float_array), axis=1), :] - rospy.logerr("After filtering inf: " + str(float_array)) if float_array.size == 0: return None # Filter out all objects that are not cars all_cars = float_array[np.where(float_array[:, 0] == 2)] - # Filter out parking cars or cars on opposite lane - no_oncoming_traffic = all_cars[np.where(all_cars[:, 6] < 0.5)] - rospy.logerr("After filtering left lane: " + str(no_oncoming_traffic)) - if no_oncoming_traffic.size == 0: + # Get cars that are on our lane + cars_in_front = all_cars[np.where(np.abs(all_cars[:, 2]) < 1.5)] + if cars_in_front.size == 0: + # no car in front return None - - no_parking_cars = no_oncoming_traffic[ - np.where(no_oncoming_traffic[:, 6] > -3)] - - if no_parking_cars.size == 0: + # Filter for potential recognition of ega vehicle front hood + filtered_cars_in_front = cars_in_front[np.where(cars_in_front[:, 1] > 0.7)] + if filtered_cars_in_front.size == 0: + # no car in front return None # Return nearest car - return no_parking_cars[np.argmin(no_parking_cars[:, 1])] + return filtered_cars_in_front[np.argmin(filtered_cars_in_front[:, 1])] From 7a4241d882701839eaa6c3eca732103a63b18a01 Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Sun, 10 Mar 2024 14:37:18 +0100 Subject: [PATCH 29/33] fix: ACC imporvements --- build/docker-compose.yml | 4 +- code/planning/launch/planning.launch | 6 +- .../src/global_planner/global_planner.py | 2 +- code/planning/src/local_planner/ACC.py | 63 ++++++++----------- .../src/local_planner/collision_check.py | 10 +-- .../src/local_planner/motion_planning.py | 4 +- 6 files changed, 41 insertions(+), 48 deletions(-) diff --git a/build/docker-compose.yml b/build/docker-compose.yml index 7b3cf6f3..1372dfdb 100644 --- a/build/docker-compose.yml +++ b/build/docker-compose.yml @@ -62,8 +62,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=paf23-carla-simulator-1 --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=paf23-carla-simulator-1 --track=MAP" logging: driver: "local" diff --git a/code/planning/launch/planning.launch b/code/planning/launch/planning.launch index 241786f0..980b6054 100644 --- a/code/planning/launch/planning.launch +++ b/code/planning/launch/planning.launch @@ -5,7 +5,7 @@ - + diff --git a/code/planning/src/global_planner/global_planner.py b/code/planning/src/global_planner/global_planner.py index be526975..23442530 100755 --- a/code/planning/src/global_planner/global_planner.py +++ b/code/planning/src/global_planner/global_planner.py @@ -84,7 +84,7 @@ def __init__(self): self.logdebug('PrePlanner-Node started') # uncomment for self.dev_load_world_info() for dev_launch - self.dev_load_world_info() + # self.dev_load_world_info() def global_route_callback(self, data: CarlaRoute) -> None: """ diff --git a/code/planning/src/local_planner/ACC.py b/code/planning/src/local_planner/ACC.py index 9ad870ae..3c9b6e0b 100755 --- a/code/planning/src/local_planner/ACC.py +++ b/code/planning/src/local_planner/ACC.py @@ -9,7 +9,7 @@ # from std_msgs.msg import String from std_msgs.msg import Float32MultiArray, Float32 from collision_check import CollisionCheck -import rospy +import numpy as np class ACC(CompatibleNode): @@ -21,7 +21,6 @@ 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 = None # m/ss # Get current speed self.velocity_sub: Subscriber = self.new_subscription( @@ -30,13 +29,6 @@ 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( - Float32, - 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, @@ -57,9 +49,9 @@ def __init__(self): callback=self.__current_position_callback, qos_profile=1) self.approx_speed_sub = self.new_subscription( - Float32, - f"/paf/{self.role_name}/cc_speed", - self.__approx_speed_callback, + Float32MultiArray, + f"/paf/{self.role_name}/collision", + self.__collision_callback, qos_profile=1) # Publish desired speed to acting self.velocity_pub: Publisher = self.new_publisher( @@ -92,15 +84,7 @@ def __init__(self): self.logdebug("ACC initialized") - def _set_distance(self, data: Float32): - """Get min distance to object in front from perception - - Args: - data (Float32): Minimum Distance from LIDAR - """ - self.obstacle_distance = data.data - - def __approx_speed_callback(self, data: Float32): + def __collision_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 @@ -108,8 +92,12 @@ def __approx_speed_callback(self, data: Float32): Args: data (Float32): Speed from obstacle in front """ - # self.logerr("ACC: Approx speed recieved: " + str(data.data)) - self.obstacle_speed = (rospy.get_rostime(), data.data) + if np.isinf(data.data[0]): + self.obstacle_speed = None + self.obstacle_distance = None + return + self.obstacle_speed = data.data[1] + self.obstacle_distance = data.data[0] def __get_current_velocity(self, data: CarlaSpeedometer): """_summary_ @@ -171,13 +159,6 @@ def loop(timer_event=None): Checks if distance to a possible object is too small and publishes the desired speed to motion planning """ - if self.obstacle_speed is not None: - # Check if too much time has passed since last speed update - if rospy.get_rostime() - self.obstacle_speed[0] < \ - rospy.Duration(1): - self.obstacle_speed = None - self.obstacle_distance = None - if self.obstacle_distance is not None and \ self.obstacle_speed is not None and \ self.__current_velocity is not None: @@ -188,23 +169,33 @@ def loop(timer_event=None): if self.obstacle_distance < safety_distance: # If safety distance is reached, we want to reduce the # speed to meet the desired distance - safe_speed = self.obstacle_speed[1] * \ + # Lerp factor: + # https://encyclopediaofmath.org/index.php?title=Linear_interpolation + safe_speed = self.obstacle_speed * \ (self.obstacle_distance / safety_distance) + lerp_factor = 0.01 + safe_speed = (1 - lerp_factor) * self.__current_velocity +\ + lerp_factor * safe_speed if safe_speed < 1.0: safe_speed = 0 + self.logerr("ACC: Safe speed: " + str(safe_speed) + + " Distance: " + str(self.obstacle_distance)) self.velocity_pub.publish(safe_speed) else: - # If safety distance is reached, drive with same speed as - # Object in front - if self.obstacle_speed[1] < 1.0: - self.obstacle_speed[1] = 0 - self.velocity_pub.publish(self.obstacle_speed[1]) + # If safety distance is reached just hold current speed + # if self.obstacle_speed < 1.0: + # self.obstacle_speed = 0 + self.logerr("ACC: my speed: " + + str(self.__current_velocity)) + self.velocity_pub.publish(self.obstacle_speed) elif self.speed_limit is not None: # If we have no obstacle, we want to drive with the current # speed limit + self.logerr("ACC: Speed limit: " + str(self.speed_limit)) self.velocity_pub.publish(self.speed_limit) else: + self.logerr("ACC: default Speed limit") self.velocity_pub.publish(5.0) self.new_timer(self.control_loop_rate, loop) diff --git a/code/planning/src/local_planner/collision_check.py b/code/planning/src/local_planner/collision_check.py index da5fef8f..3c4adb31 100755 --- a/code/planning/src/local_planner/collision_check.py +++ b/code/planning/src/local_planner/collision_check.py @@ -68,6 +68,9 @@ def update_distance(self, reset): # Reset all values if we do not have car in front self.__object_last_position = None self.__object_first_position = None + # Signal to ACC to reset speed and distance + data = Float32MultiArray(data=[np.Inf, np.Inf]) + self.collision_pub.publish(data) return if self.__object_first_position is None: self.__object_first_position = self.__object_last_position @@ -89,7 +92,7 @@ def __set_distance(self, data: Float32MultiArray): return elif nearest_object is None: return - self.logerr("Obstacle detected: " + str(nearest_object)) + # 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] < \ @@ -173,11 +176,12 @@ def calculate_rule_of_thumb(emergency, speed): Args: emergency (bool): if emergency brake is initiated + speed (float): speed of the vehicle (km/h) Returns: float: distance calculated with rule of thumb """ - reaction_distance = speed + reaction_distance = 0 braking_distance = (speed * 0.36)**2 if emergency: return reaction_distance + braking_distance / 2 @@ -206,11 +210,9 @@ def check_crash(self, obstacle): self.emergency_pub.publish(True) # When no emergency brake is needed publish collision object data = Float32MultiArray(data=[distance, obstacle_speed]) - self.logerr("Collision published") self.collision_pub.publish(data) else: # If no collision is ahead publish np.Inf - self.logerr("No collision") data = Float32MultiArray(data=[np.Inf, obstacle_speed]) self.collision_pub.publish(data) diff --git a/code/planning/src/local_planner/motion_planning.py b/code/planning/src/local_planner/motion_planning.py index fc6baed7..16f2364a 100755 --- a/code/planning/src/local_planner/motion_planning.py +++ b/code/planning/src/local_planner/motion_planning.py @@ -260,7 +260,7 @@ def change_trajectory(self, distance_obj): result + pose_list[int(self.current_wp + distance_obj + 30):] - self.trajectory = path + # self.trajectory = path else: self.logerr("Overtake failed") self.overtake_success_pub.publish(-1) @@ -393,7 +393,7 @@ def update_target_speed(self, acc_speed, behavior): be_speed = self.get_speed_by_behavior(behavior) if not behavior == bs.parking.name: corner_speed = self.get_cornering_speed() - self.target_speed = min(be_speed, acc_speed, corner_speed, 6) + self.target_speed = min(be_speed, acc_speed, corner_speed) else: self.target_speed = be_speed # self.target_speed = min(self.target_speed, 8) From 54d69dbaa4aaa12d312f4f0e300eef7fb1feea5a Mon Sep 17 00:00:00 2001 From: JuliusMiller Date: Sun, 10 Mar 2024 14:40:01 +0100 Subject: [PATCH 30/33] fix: update for overtake --- .../src/behavior_agent/behaviours/overtake.py | 24 +++++-- .../src/local_planner/motion_planning.py | 68 ++++++++++++++++--- 2 files changed, 77 insertions(+), 15 deletions(-) diff --git a/code/planning/src/behavior_agent/behaviours/overtake.py b/code/planning/src/behavior_agent/behaviours/overtake.py index ca508734..3d479328 100644 --- a/code/planning/src/behavior_agent/behaviours/overtake.py +++ b/code/planning/src/behavior_agent/behaviours/overtake.py @@ -92,6 +92,10 @@ def update(self): self.ot_distance = _dis.data[0] rospy.loginfo(f"Overtake distance: {self.ot_distance}") + if np.isinf(self.ot_distance): + rospy.loginfo("Abort Overtake") + return py_trees.common.Status.FAILURE + # slow down before overtake if blocked if self.ot_distance < 15.0: distance_lidar = self.blackboard. \ @@ -123,7 +127,7 @@ def update(self): rospy.loginfo("still approaching") return py_trees.common.Status.RUNNING elif speed < convert_to_ms(2.0) and \ - self.ot_distance < 6.0: + self.ot_distance < 8.0: # stopped rospy.loginfo("stopped") return py_trees.common.Status.SUCCESS @@ -241,7 +245,7 @@ def update(self): return py_trees.common.Status.FAILURE else: rospy.loginfo("No Lidar Distance") - return py_trees.common.Success + return py_trees.common.Status.SUCCESS def terminate(self, new_status): """ @@ -328,11 +332,11 @@ def update(self): rospy.loginfo("Overtake: Slowing down") return py_trees.common.Status.RUNNING else: - rospy.loginfo("Overtake: Big Failure") + rospy.loginfo("Overtake: Abort ") return py_trees.common.Status.FAILURE else: rospy.loginfo("Overtake: Bigger Failure") - return py_trees.common.Status.RUNNING + return py_trees.common.Status.FAILURE # Currently not in use # Can be used to check if we can go back to the original lane @@ -394,6 +398,9 @@ def initialise(self): """ 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]) return True def update(self): @@ -407,7 +414,14 @@ def update(self): Abort this subtree :return: py_trees.common.Status.FAILURE, to exit this subtree """ - return py_trees.common.Status.FAILURE + 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: + return py_trees.common.Status.FAILURE + else: + return py_trees.common.Status.RUNNING def terminate(self, new_status): """ diff --git a/code/planning/src/local_planner/motion_planning.py b/code/planning/src/local_planner/motion_planning.py index fc6baed7..ff29a21a 100755 --- a/code/planning/src/local_planner/motion_planning.py +++ b/code/planning/src/local_planner/motion_planning.py @@ -11,6 +11,8 @@ from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion from carla_msgs.msg import CarlaSpeedometer import numpy as np +from scipy.spatial.transform import Rotation +import math from frenet_optimal_trajectory_planner.FrenetOptimalTrajectory.fot_wrapper \ import run_fot @@ -230,6 +232,11 @@ def change_trajectory(self, distance_obj): pose_list = self.trajectory.poses count_retrys = 0 + # Only use fallback + self.overtake_fallback(distance_obj, pose_list) + self.overtake_success_pub.publish(1) + return + success_overtake = False while not success_overtake and count_retrys < 10: result_x, result_y, speeds, ix, iy, iyaw, d, s, speeds_x, \ @@ -263,7 +270,50 @@ def change_trajectory(self, distance_obj): self.trajectory = path else: self.logerr("Overtake failed") - self.overtake_success_pub.publish(-1) + self.overtake_fallback(distance_obj, pose_list) + self.overtake_success_pub.publish(1) + + def overtake_fallback(self, distance, pose_list): + self.logerr("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] + waypoints = self.convert_pose_to_array(selection) + + offset = np.array([2, 0, 0]) + rotation_adjusted = Rotation.from_euler('z', self.current_heading + + math.radians(90)) + offset_front = rotation_adjusted.apply(offset) + 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 = [] + 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]) + orientation = Quaternion(x=0, y=0, + z=0, w=0) + 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 + 10):] + self.trajectory = path def __set_trajectory(self, data: Path): """get current trajectory global planning @@ -332,7 +382,7 @@ def map_corner(dist): if dist < 8: # lane_change return 8 elif dist < 25: - return 6 + return 4 # 6 elif dist < 50: return 7 else: @@ -393,7 +443,7 @@ def update_target_speed(self, acc_speed, behavior): be_speed = self.get_speed_by_behavior(behavior) if not behavior == bs.parking.name: corner_speed = self.get_cornering_speed() - self.target_speed = min(be_speed, acc_speed, corner_speed, 6) + self.target_speed = min(be_speed, acc_speed, corner_speed) else: self.target_speed = be_speed # self.target_speed = min(self.target_speed, 8) @@ -407,6 +457,9 @@ def __set_acc_speed(self, data: Float32): 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) + return self.change_trajectory(self.__collision_point) def __set_stopline(self, data: Waypoint) -> float: @@ -421,10 +474,6 @@ def __set_change_point(self, data: LaneChange): def __set_collision_point(self, data: Float32MultiArray): if data.data is not None: self.__collision_point = data.data[0] - if self.__collision_point != np.inf and not self.calculated: - self.change_trajectory(self.__collision_point) - self.traj_pub.publish(self.trajectory) - self.calculated = True def get_speed_by_behavior(self, behavior: str) -> float: speed = 0.0 @@ -526,12 +575,11 @@ def __calc_speed_to_stop_lanechange(self) -> float: def __calc_speed_to_stop_overtake(self) -> float: stopline = self.__calc_virtual_overtake() - self.logerr(stopline) v_stop = max(convert_to_ms(10.), convert_to_ms((stopline / 30) * 50)) - if stopline < 6.0: + if stopline < 8.0: v_stop = 0.0 return v_stop @@ -548,7 +596,7 @@ def __calc_virtual_stopline(self) -> float: return 0.0 def __calc_virtual_overtake(self) -> float: - self.logerr(f"Overtake point: {self.__collision_point}") + # self.logerr(f"Overtake point: {self.__collision_point}") if (self.__collision_point is not None) and \ self.__collision_point != np.inf: return self.__collision_point From b7cd586d539c518b4cc8991e963ae1032b3f04cc Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Sun, 10 Mar 2024 16:34:17 +0100 Subject: [PATCH 31/33] fix: Overtaking adjusted --- code/planning/launch/planning.launch | 2 +- .../src/behavior_agent/behaviours/overtake.py | 2 +- .../behaviours/road_features.py | 31 +++++++++++++++++-- .../behaviours/topics2blackboard.py | 2 ++ code/planning/src/local_planner/ACC.py | 13 ++++---- .../src/local_planner/collision_check.py | 2 ++ .../src/local_planner/motion_planning.py | 12 +++---- code/planning/src/local_planner/utils.py | 2 +- 8 files changed, 48 insertions(+), 18 deletions(-) diff --git a/code/planning/launch/planning.launch b/code/planning/launch/planning.launch index 980b6054..7437344e 100644 --- a/code/planning/launch/planning.launch +++ b/code/planning/launch/planning.launch @@ -5,7 +5,7 @@ - +