From 75faf91697e9188e660f5d8c2313da3341a7bc80 Mon Sep 17 00:00:00 2001 From: hellschwalex Date: Thu, 25 Jan 2024 19:33:00 +0100 Subject: [PATCH 01/43] feat: tune stanley and compare with purepursuit --- code/acting/src/acting/Acting_DebuggerNode.py | 95 +++++++++++++++---- code/acting/src/acting/stanley_controller.py | 50 +++------- code/acting/src/acting/vehicle_controller.py | 12 ++- code/acting/src/acting/velocity_controller.py | 27 +++--- 4 files changed, 109 insertions(+), 75 deletions(-) diff --git a/code/acting/src/acting/Acting_DebuggerNode.py b/code/acting/src/acting/Acting_DebuggerNode.py index 9de92f6c..c162c2e7 100755 --- a/code/acting/src/acting/Acting_DebuggerNode.py +++ b/code/acting/src/acting/Acting_DebuggerNode.py @@ -20,6 +20,7 @@ import rospy from rospy import Publisher, Subscriber from carla_msgs.msg import CarlaSpeedometer, CarlaEgoVehicleControl +from acting.msg import StanleyDebug from trajectory_interpolation import interpolate_route @@ -40,7 +41,7 @@ # TURN OFF stanley and PP Controllers in acting.launch! # 2: Test Steering Controller on chooseable trajectory -# velocity = TARGET_VELOCITY_1 TODO: maybe use velocity publisher? +# velocity = TARGET_VELOCITY_1 # steering = STEERING_CONTROLLER_USED (see below) # trajectory = TRAJECTORY_TYPE (see below) @@ -55,14 +56,15 @@ # TODO TODO TEST_TYPE = 2 # aka. TT -FIXED_STEERING: float = 0 # for TT0: steering 0.0 = always straight -TARGET_VELOCITY_1: float = 5 # for TT0/TT1: low velocity -TARGET_VELOCITY_2: float = 0 # for TT1: high velocity +FIXED_STEERING: float = 0 # for TestType0 fixed Steering +TARGET_VELOCITY_1: float = 20 # velocity 1 +TARGET_VELOCITY_2: float = 0 # velocity 2 -STEERING_CONTROLLER_USED = 1 # for TT2: 0 = both ; 1 = PP ; 2 = Stanley -TRAJECTORY_TYPE = 2 # for TT2: 0 = Straight ; 1 = Curve ; 2 = SineWave +STEERING_CONTROLLER_USED = 0 # 0 = both ; 1 = PP ; 2 = Stanley +# 0 = Straight ; 1 = Curve ; 2 = SineWave ; 3 = Overtake +TRAJECTORY_TYPE = 3 -PRINT_AFTER_TIME = 20.0 # How long after Simulationstart to print data +PRINT_AFTER_TIME = 7.0 # How long after Simulationstart to print data class Acting_Debugger(CompatibleNode): @@ -112,7 +114,7 @@ def __init__(self): f"/paf/{self.role_name}/controller_selector_debug", qos_profile=1) - # Subscriber of current_pos, used for TODO nothing yet + # Subscriber of current_pos, used for Steering Debugging self.current_pos_sub: Subscriber = self.new_subscription( msg_type=PoseStamped, topic="/paf/" + self.role_name + "/current_pos", @@ -132,8 +134,7 @@ def __init__(self): Float32, f"/paf/{self.role_name}/current_heading", self.__get_heading, - qos_profile=1 - ) + qos_profile=1) # Subscriber for current_velocity for plotting self.current_velocity_sub: Subscriber = self.new_subscription( @@ -163,7 +164,14 @@ def __init__(self): self.__get_stanley_steer, qos_profile=1) - # Subscriber for Stanley_Steer + # Subscriber for StanleyDebug + self.stanley_debug_sub: Subscriber = self.new_subscription( + StanleyDebug, + f"/paf/{self.role_name}/stanley_debug", + self.__get_stanley_debug, + qos_profile=1) + + # Subscriber for vehicle_steer self.vehicle_steer_sub: Subscriber = self.new_subscription( CarlaEgoVehicleControl, f'/carla/{self.role_name}/vehicle_control_cmd', @@ -192,12 +200,13 @@ def __init__(self): self.__throttles = [] self.__current_headings = [] - self.__yaws = [] self.__purepursuit_steers = [] self.__stanley_steers = [] self.__vehicle_steers = [] + self.stanley_cross_errors = [] + self.positions = [] self.path_msg = Path() @@ -205,7 +214,7 @@ def __init__(self): self.path_msg.header.frame_id = "global" # Generate Trajectory as selected in TRAJECTORY_TYPE - # Spawncoords at the simulationstart TODO: get from position + # Spawncoords at the simulationstart startx = 984.5 starty = -5442.0 @@ -242,7 +251,7 @@ def __init__(self): length = np.pi * 2 * cycles step = length / resolution # spacing between values my_wave = np.sin(np.arange(0, length, step)) - x_wave = 0.15 * my_wave # to have a serpentine line with +/-1.5 m + x_wave = 1.5 * my_wave # to have a serpentine line with +/-1.5 m # to have the serpentine line drive around the middle # of the road/start point of the car x_wave += startx @@ -260,6 +269,50 @@ def __init__(self): self.current_trajectory = trajectory_wave self.updated_trajectory(self.current_trajectory) + elif (TRAJECTORY_TYPE == 3): # 2 Lane Switches + self.current_trajectory = [ + (startx, starty), + (startx-0.5, starty-10), + (startx-0.5, starty-20), + + (startx-0.4, starty-21), + (startx-0.3, starty-22), + (startx-0.2, starty-23), + (startx-0.1, starty-24), + (startx, starty-25), + (startx+0.1, starty-26), + (startx+0.2, starty-27), + (startx+0.3, starty-28), + (startx+0.4, starty-29), + (startx+0.5, starty-30), + (startx+0.6, starty-31), + (startx+0.7, starty-32), + (startx+0.8, starty-33), + (startx+0.9, starty-34), + (startx+1.0, starty-35), + (startx+1.0, starty-50), + + (startx+1.0, starty-51), + (startx+0.9, starty-52), + (startx+0.8, starty-53), + (startx+0.7, starty-54), + (startx+0.6, starty-55), + (startx+0.5, starty-56), + (startx+0.4, starty-57), + (startx+0.3, starty-58), + (startx+0.2, starty-59), + (startx+0.1, starty-60), + (startx, starty-61), + (startx-0.1, starty-62), + (startx-0.2, starty-63), + (startx-0.3, starty-64), + (startx-0.4, starty-65), + (startx-0.5, starty-66), + + (startx-0.5, starty-100), + ] + self.updated_trajectory(self.current_trajectory) + def updated_trajectory(self, target_trajectory): """ Updates the published Path message with the new target trajectory. @@ -277,7 +330,7 @@ def updated_trajectory(self, target_trajectory): pos.header.frame_id = "global" pos.pose.position.x = wp[0] pos.pose.position.y = wp[1] - pos.pose.position.z = 35 # why?? + pos.pose.position.z = 704 # why?? # currently not used therefore zeros pos.pose.orientation.x = 0 pos.pose.orientation.y = 0 @@ -298,9 +351,6 @@ def __current_position_callback(self, data: PoseStamped): def __get_heading(self, data: Float32): self.__current_headings.append(float(data.data)) - def __get_yaw(self, data: Float32): - self.__yaws.append(float(data.data)) - def __get_target_velocity(self, data: Float32): self.__max_velocities.append(float(data.data)) @@ -313,6 +363,9 @@ def __get_throttle(self, data: Float32): def __get_stanley_steer(self, data: Float32): self.__stanley_steers.append(float(data.data)) + def __get_stanley_debug(self, data: StanleyDebug): + self.stanley_cross_errors.append(data.cross_err) + def __get_purepursuit_steer(self, data: Float32): r = 1 / (math.pi / 2) steering_float = float(data.data) * r @@ -399,7 +452,7 @@ def loop(timer_event=None): self.checkpoint_time = rospy.get_time() self.time_set = True print(">>>>>>>>>>>> TRAJECTORY <<<<<<<<<<<<<<") - # print(self.current_trajectory) + print(self.current_trajectory) print(">>>>>>>>>>>> TRAJECTORY <<<<<<<<<<<<<<") # Uncomment the prints of the data you want to plot @@ -413,8 +466,8 @@ def loop(timer_event=None): # print(self.__stanley_steers) # print(self.__vehicle_steers) # print(self.__current_headings) - # print(self.__yaws) - # print(self.positions) + print(self.positions) + # print(self.stanley_cross_errors) print(">>>>>>>>>>>> DATA <<<<<<<<<<<<<<") self.new_timer(self.control_loop_rate, loop) diff --git a/code/acting/src/acting/stanley_controller.py b/code/acting/src/acting/stanley_controller.py index e03e7817..f325ff73 100755 --- a/code/acting/src/acting/stanley_controller.py +++ b/code/acting/src/acting/stanley_controller.py @@ -15,6 +15,8 @@ from helper_functions import vector_angle from trajectory_interpolation import points_to_vector +K_CROSSERR = 1.24 + class StanleyController(CompatibleNode): def __init__(self): @@ -60,19 +62,6 @@ def __init__(self): f"/paf/{self.role_name}/stanley_debug", qos_profile=1) - # Publishers for debugging StanleyController soon - # TODO: only works with perfect current_pos - # publish the target and the current position - self.targetwp_publisher: Publisher = self.new_publisher( - Float32, - f"/paf/{self.role_name}/current_target_wp", - qos_profile=1) - - self.currentx_publisher: Publisher = self.new_publisher( - Float32, - f"/paf/{self.role_name}/current_x", - qos_profile=1) - self.__position: (float, float) = None # x , y self.__last_pos: (float, float) = None # x , y self.__path: Path = None @@ -169,29 +158,23 @@ def __calculate_steer(self) -> float: using the Stanley algorithm :return: steering angle """ - # TODO: tune both next sprint - k_ce = 0.10 - k_v = 1.0 - - current_velocity: float - if self.__velocity <= 1: - current_velocity = 1 - else: - current_velocity = self.__velocity + # do not allow dividing by 0 or < 1 + if self.__velocity < 1: + return 0 + current_velocity: float = self.__velocity closest_point_idx = self.__get_closest_point_index() - closest_point: PoseStamped = self.__path.poses[closest_point_idx] + # calculate heading_err from current_heading and traj_heading traj_heading = self.__get_path_heading(closest_point_idx) - - cross_err = self.__get_cross_err(closest_point.pose.position) heading_err = self.__heading - traj_heading heading_err = ((heading_err + math.pi) % (2 * math.pi)) - math.pi - - steering_angle = heading_err + atan((k_ce * cross_err) / - current_velocity * k_v) - steering_angle *= - 1 - - # for debugging TODO: currently not that useful-> + # calculate cross_err from current_position and closest traj_point + closest_point: PoseStamped = self.__path.poses[closest_point_idx] + cross_err = self.__get_cross_err(closest_point.pose.position) + # * -1 because it is inverted compared to PurePursuit + steering_angle = -1 * (heading_err + atan((K_CROSSERR * cross_err) + / current_velocity)) + # -> for debugging debug_msg = StanleyDebug() debug_msg.heading = self.__heading debug_msg.path_heading = traj_heading @@ -199,10 +182,7 @@ def __calculate_steer(self) -> float: debug_msg.heading_err = heading_err debug_msg.steering_angle = steering_angle self.debug_publisher.publish(debug_msg) - # <- - # 2 more debugging messages: TODO: maybe put into DEBUGGER NODE? - self.targetwp_publisher.publish((closest_point.pose.position.x-984.5)) - self.currentx_publisher.publish(self.__position[0]-984.5) + # for debugging <- return steering_angle def __get_closest_point_index(self) -> int: diff --git a/code/acting/src/acting/vehicle_controller.py b/code/acting/src/acting/vehicle_controller.py index e210ec76..338a23e3 100755 --- a/code/acting/src/acting/vehicle_controller.py +++ b/code/acting/src/acting/vehicle_controller.py @@ -176,10 +176,14 @@ def loop(timer_event=None) -> None: f_pure_p = (1 - p_stanley) * self.__pure_pursuit_steer steer = f_stanley + f_pure_p - # only use pure_pursuit controller for now, since - # stanley seems broken with the new heading-bug - # TODO: swap back if stanley is fixed - # steer = self.__pure_pursuit_steer + # -- Hardcode steering to only use Stanley -- + # ---------------- + steer = self.__pure_pursuit_steer + # steer = self.__stanley_steer + # steer = 0.5 * self.__stanley_steer + # + 0.5 * self.__pure_pursuit_steer + # ---------------- + # -- Hardcode steering to only use Stanley -- self.target_steering_publisher.publish(steer) # debugging diff --git a/code/acting/src/acting/velocity_controller.py b/code/acting/src/acting/velocity_controller.py index f00962cc..11818caa 100755 --- a/code/acting/src/acting/velocity_controller.py +++ b/code/acting/src/acting/velocity_controller.py @@ -8,7 +8,7 @@ from nav_msgs.msg import Path # TODO put back to 36 when controller can handle it -SPEED_LIMIT_DEFAULT: float = 7 # 36.0 +SPEED_LIMIT_DEFAULT: float = 0 # 36.0 class VelocityController(CompatibleNode): @@ -105,22 +105,19 @@ def loop(timer_event=None): self.logerr("VelocityController doesn't support backward " "driving yet.") return - - v = self.__target_velocity - - pid_t.setpoint = v - throttle = pid_t(self.__current_velocity) - - # pid_b.setpoint = v - # brake = pid_b(self.__current_velocity) - - # use negative throttles as brake inputs, works OK - if throttle < 0: - brake = abs(throttle) + # very low target_velocities -> stand + if self.__target_velocity < 1: + brake = 1 throttle = 0 else: - brake = 0 - + v = self.__target_velocity + pid_t.setpoint = v + throttle = pid_t(self.__current_velocity) + if throttle < 0: + brake = abs(throttle) + throttle = 0 + else: + brake = 0 self.brake_pub.publish(brake) self.throttle_pub.publish(throttle) From 96669327d09bdd2d26ea68917bd599f7704db507 Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Tue, 30 Jan 2024 14:58:57 +0100 Subject: [PATCH 02/43] 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 03/43] 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 04/43] 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 05/43] 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 06/43] 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 07/43] 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 08/43] 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 09/43] 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 10/43] 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 11/43] 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 12/43] 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 13/43] 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 14/43] 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 15/43] 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 16/43] 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 17/43] 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 18/43] 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 19/43] 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 20/43] 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 21/43] 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 22/43] 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 23/43] 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 24/43] 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 25/43] 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 26/43] 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 27/43] 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 fedfea98ef810284daca8fce5f740a11b9902884 Mon Sep 17 00:00:00 2001 From: ll7 Date: Fri, 8 Mar 2024 12:32:09 +0100 Subject: [PATCH 28/43] docs(#204): add documentation requirements Add documentation requirements for the evaluation of paf23. --- .../13_documentation_requirements.md | 45 +++++++++++++++++++ 1 file changed, 45 insertions(+) create mode 100644 doc/02_development/13_documentation_requirements.md diff --git a/doc/02_development/13_documentation_requirements.md b/doc/02_development/13_documentation_requirements.md new file mode 100644 index 00000000..4b06ab81 --- /dev/null +++ b/doc/02_development/13_documentation_requirements.md @@ -0,0 +1,45 @@ +# Documentation Requirements + +## Author + +Lennart Luttkus + +## Date + +08.03.2024 + +--- + +1. **Readability and Maintainability:** + - **Consistent Formatting:** Code should follow a consistent and readable formatting style. Tools like linters or formatters can help enforce a consistent code style. + - [02_linting](./02_linting.md) + - [04_coding_style](./04_coding_style.md) + - **Meaningful Names:** Variable and function names should be descriptive and convey the purpose of the code. + - **Comments:** Clear and concise comments should be used where necessary to explain complex logic or provide context. +2. **Code Structure:** + - **Modularity:** Code should be organized into modular components or functions, promoting reusability and maintainability. + - **Appropriate Use of Functions/Methods:** Functions should have a clear purpose and adhere to the single responsibility principle. + - **Hierarchy and Nesting:** Avoid overly nested structures; use appropriate levels of indentation to enhance readability. +3. **Efficiency and Performance:** + - **Optimized Algorithms:** Code should use efficient algorithms and data structures to achieve good performance. + - **Avoidance of Code Smells:** Detect and eliminate code smells such as duplicated code, unnecessary complexity, or anti-patterns. +4. **Error Handling:** + - **Effective Error Messages:** Error messages should be clear and provide useful information for debugging. + - **Graceful Error Handling:** Code should handle errors gracefully, avoiding crashes and providing appropriate feedback. +5. **Testing:**? + - **Comprehensive Test Coverage:** Code should be accompanied by a suite of tests that cover different scenarios, ensuring reliability and maintainability. + - **Test Readability:** Tests should be clear and easy to understand, serving as documentation for the codebase. +6. **Security:** + - **Input Validation:** Code should validate and sanitize inputs. +7. **Documentation:** + - **Code Comments:** In addition to in-code comments, consider external documentation for the overall project structure, APIs, and configurations. + - **README Files:** Include a well-written README file that provides an overview of the project, installation instructions, and usage examples. +8. **Version Control:** + - **Commit Messages:** Use descriptive and meaningful commit messages to track changes effectively. + - [03_commit](./03_commit.md) + - **Branching Strategy:** Follow a consistent and well-defined branching strategy to manage code changes. +9. **Scalability:** + - **Avoid Hardcoding:** Parameterize values that might change, making it easier to scale the application. + - **Optimized Resource Usage:** Ensure efficient utilization of resources to support scalability. +10. **Consistency with Coding Standards:** + - **Adherence to Coding Guidelines:** Follow established coding standards and best practices for the programming language or framework used. From fbc4a7cf091851f172aa3de04724eddb0a359ba3 Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Fri, 8 Mar 2024 13:48:57 +0100 Subject: [PATCH 29/43] 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 30/43] 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 9cd77a479812a88ff7f625a5fca7e5dde5381cf1 Mon Sep 17 00:00:00 2001 From: hellschwalex Date: Sat, 9 Mar 2024 13:51:47 +0100 Subject: [PATCH 31/43] feat: Clean up Acting --- code/acting/launch/acting.launch | 41 +- code/acting/src/acting/Acting_Debug_Node.py | 420 ++++++++++++++++++ .../Acting_Debug_Node_OLD_WITH_STANLEY.py} | 53 +-- code/acting/src/acting/{ => OBSOLETE}/acc.py | 0 .../acc_distance_publisher_dummy.py | 0 .../acting_velocity_publisher.py | 0 .../{ => OBSOLETE}/stanley_controller.py | 10 +- ...cle_controller_OLD_WITH_STANLEY_AND_PID.py | 317 +++++++++++++ .../src/acting/pure_pursuit_controller.py | 29 +- code/acting/src/acting/vehicle_controller.py | 143 +----- 10 files changed, 807 insertions(+), 206 deletions(-) create mode 100755 code/acting/src/acting/Acting_Debug_Node.py rename code/acting/src/acting/{Acting_DebuggerNode.py => OBSOLETE/Acting_Debug_Node_OLD_WITH_STANLEY.py} (93%) rename code/acting/src/acting/{ => OBSOLETE}/acc.py (100%) rename code/acting/src/acting/{ => OBSOLETE}/acc_distance_publisher_dummy.py (100%) rename code/acting/src/acting/{ => OBSOLETE}/acting_velocity_publisher.py (100%) rename code/acting/src/acting/{ => OBSOLETE}/stanley_controller.py (96%) create mode 100755 code/acting/src/acting/OBSOLETE/vehicle_controller_OLD_WITH_STANLEY_AND_PID.py diff --git a/code/acting/launch/acting.launch b/code/acting/launch/acting.launch index 3a705e5d..3a899397 100644 --- a/code/acting/launch/acting.launch +++ b/code/acting/launch/acting.launch @@ -3,56 +3,59 @@ - - - + - + - + + - - + + + - + + - + + --> - - - + diff --git a/code/acting/src/acting/Acting_Debug_Node.py b/code/acting/src/acting/Acting_Debug_Node.py new file mode 100755 index 00000000..289c58d7 --- /dev/null +++ b/code/acting/src/acting/Acting_Debug_Node.py @@ -0,0 +1,420 @@ +#!/usr/bin/env python + +""" +This node provides full testability for all Acting +components by offering different testcases +to hopefully fully implement and tune Acting without +the need of working Perception and Planning components. +This also generates Lists of all the important values +to be saved in a file and plotted again. +TODO: emergency brake behavior +""" + +import math +import ros_compatibility as roscomp +import numpy as np +from nav_msgs.msg import Path +from std_msgs.msg import Float32, Bool +from geometry_msgs.msg import PoseStamped +from ros_compatibility.node import CompatibleNode +import rospy +from rospy import Publisher, Subscriber +from carla_msgs.msg import CarlaSpeedometer, CarlaEgoVehicleControl + +from trajectory_interpolation import interpolate_route + +# TEST_TYPE to choose which kind of Test to run: +# 0: Test Velocity Controller with constant one velocity +# const. velocity = TARGET_VELOCITY_1 +# const. steering = 0 +# no trajectory +# TURN OFF PP Controller in acting.launch! + +# 1: Test Velocity Controller with changing velocity +# velocity = alternate all 20 secs: TARGET_VELOCITY_1/_2 +# const. steering = 0 +# no trajectory +# TURN OFF PP Controller in acting.launch! + +# 2: Test Steering Controller on chooseable trajectory +# velocity = TARGET_VELOCITY_1 +# steering = STEERING_CONTROLLER_USED (see below) +# trajectory = TRAJECTORY_TYPE (see below) + +# 3: Test Emergency Breaks on TestType 1 +# const velocity = TARGET_VELOCITY_1 +# const steering = 0 +# no trajectory +# Triggers emergency break after 15 Seconds +TEST_TYPE = 2 +FIXED_STEERING: float = 0 # if fixed steering needed +TARGET_VELOCITY_1: float = 10 # standard velocity +TARGET_VELOCITY_2: float = 0 # second velocity to switch to +# 0 = Straight ; 1 = Curve ; 2 = SineWave ; 3 = Overtake +TRAJECTORY_TYPE = 3 +PRINT_AFTER_TIME = 10.0 # How long after Simulationstart to print data + + +class Acting_Debug_Node(CompatibleNode): + """ + Creates a node with testability for all acting components + without the need of working/running perception or planning. + """ + + def __init__(self): + """ + Constructor of the class + :return: + """ + super(Acting_Debug_Node, self).__init__('dummy_trajectory_pub') + self.loginfo('Acting_Debug_Node node started') + self.role_name = self.get_param('role_name', 'ego_vehicle') + self.control_loop_rate = self.get_param('control_loop_rate', 0.05) + + # Publisher for Dummy Trajectory + self.trajectory_pub: Publisher = self.new_publisher( + Path, + "/paf/" + self.role_name + "/trajectory", + qos_profile=1) + + # Publisher for Dummy Velocity + self.velocity_pub: Publisher = self.new_publisher( + Float32, + f"/paf/{self.role_name}/target_velocity", + qos_profile=1) + + # PurePursuit: Publisher for Dummy PP-Steer + self.pure_pursuit_steer_pub: Publisher = self.new_publisher( + Float32, + f"/paf/{self.role_name}/pure_pursuit_steer", + qos_profile=1) + + # Subscriber of current_pos, used for Steering Debugging + self.current_pos_sub: Subscriber = self.new_subscription( + msg_type=PoseStamped, + topic="/paf/" + self.role_name + "/current_pos", + callback=self.__current_position_callback, + qos_profile=1) + + # ---> EVALUATION/TUNING: Subscribers for plotting + # Subscriber for target_velocity for plotting + self.target_velocity_sub: Subscriber = self.new_subscription( + Float32, + f"/paf/{self.role_name}/target_velocity", + self.__get_target_velocity, + qos_profile=1) + + # Subscriber for current_headng for plotting + self.heading_sub: Subscriber = self.new_subscription( + Float32, + f"/paf/{self.role_name}/current_heading", + self.__get_heading, + qos_profile=1) + + # Subscriber for current_velocity for plotting + self.current_velocity_sub: Subscriber = self.new_subscription( + CarlaSpeedometer, + f"/carla/{self.role_name}/Speed", + self.__get_current_velocity, + qos_profile=1) + + # Subscriber for current_throttle for plotting + self.current_throttle_sub: Subscriber = self.new_subscription( + Float32, + f"/paf/{self.role_name}/throttle", + self.__get_throttle, + qos_profile=1) + + # Subscriber for PurePursuit_steer + self.pure_pursuit_steer_sub: Subscriber = self.new_subscription( + Float32, + f"/paf/{self.role_name}/pure_pursuit_steer", + self.__get_purepursuit_steer, + qos_profile=1) + + # Subscriber for vehicle_steer + self.vehicle_steer_sub: Subscriber = self.new_subscription( + CarlaEgoVehicleControl, + f'/carla/{self.role_name}/vehicle_control_cmd', + self.__get_vehicle_steer, + qos_profile=10) + + # Publisher for emergency brake testing + self.emergency_pub: Publisher = self.new_publisher( + Bool, + f"/paf/{self.role_name}/emergency", + qos_profile=1) + + # Initialize all needed "global" variables here + self.current_trajectory = [] + self.switchVelocity = False + self.driveVel = TARGET_VELOCITY_1 + self.switch_checkpoint_time = rospy.get_time() + self.switch_time_set = False + self.checkpoint_time = rospy.get_time() + self.time_set = False + self.__current_velocities = [] + self.__max_velocities = [] + self.__throttles = [] + self.__current_headings = [] + self.__purepursuit_steers = [] + self.__vehicle_steers = [] + self.stanley_cross_errors = [] + self.positions = [] + + # Generate Trajectory as selected in TRAJECTORY_TYPE + self.path_msg = Path() + self.path_msg.header.stamp = rospy.Time.now() + self.path_msg.header.frame_id = "global" + # Spawncoords at the simulationstart + startx = 984.5 + starty = -5442.0 + if (TRAJECTORY_TYPE == 0): # Straight trajectory + self.current_trajectory = [ + (startx, starty), + (startx, starty-200) + ] + + elif (TRAJECTORY_TYPE == 1): # straight into 90° Curve + self.current_trajectory = [ + (984.5, -5442.0), + + (984.5, -5563.5), + (985.0, -5573.2), + (986.3, -5576.5), + (987.3, -5578.5), + (988.7, -5579.0), + (990.5, -5579.8), + (1000.0, -5580.2), + + (1040.0, -5580.0), + (1070.0, -5580.0) + ] + + elif (TRAJECTORY_TYPE == 2): # Sinewave Serpentines trajectory + # Generate a sine-wave with the global Constants to + # automatically generate a trajectory with serpentine waves + cycles = 4 # how many sine cycles + resolution = 70 # how many datapoints to generate + + length = np.pi * 2 * cycles + step = length / resolution # spacing between values + my_wave = np.sin(np.arange(0, length, step)) + x_wave = 1.5 * my_wave # to have a serpentine line with +/-1.5 m + # to have the serpentine line drive around the middle + # of the road/start point of the car + x_wave += startx + traj_y = starty + # starting point in the middle of the road, + # sine wave swings around this later + trajectory_wave = [(startx, traj_y)] + for traj_x in x_wave: + traj_y -= 2 + trajectory_wave.append((traj_x, traj_y)) + # back to the middle of the road + trajectory_wave.append((startx, traj_y-2)) + # add a long straight path after the serpentines + trajectory_wave.append((startx, starty-200)) + self.current_trajectory = trajectory_wave + + elif (TRAJECTORY_TYPE == 3): # 2 Lane Switches + self.current_trajectory = [ + (startx, starty), + (startx-0.5, starty-10), + (startx-0.5, starty-20), + + (startx-0.4, starty-21), + (startx-0.3, starty-22), + (startx-0.2, starty-23), + (startx-0.1, starty-24), + (startx, starty-25), + (startx+0.1, starty-26), + (startx+0.2, starty-27), + (startx+0.3, starty-28), + (startx+0.4, starty-29), + (startx+0.5, starty-30), + (startx+0.6, starty-31), + (startx+0.7, starty-32), + (startx+0.8, starty-33), + (startx+0.9, starty-34), + (startx+1.0, starty-35), + (startx+1.0, starty-50), + + (startx+1.0, starty-51), + (startx+0.9, starty-52), + (startx+0.8, starty-53), + (startx+0.7, starty-54), + (startx+0.6, starty-55), + (startx+0.5, starty-56), + (startx+0.4, starty-57), + (startx+0.3, starty-58), + (startx+0.2, starty-59), + (startx+0.1, starty-60), + (startx, starty-61), + (startx-0.1, starty-62), + (startx-0.2, starty-63), + (startx-0.3, starty-64), + (startx-0.4, starty-65), + (startx-0.5, starty-66), + + (startx-0.5, starty-100), + ] + self.updated_trajectory(self.current_trajectory) + + def updated_trajectory(self, target_trajectory): + """ + Updates the published Path message with the new target trajectory. + :param: target_trajectory: the new target trajectory to be published + :return: + """ + self.current_trajectory = interpolate_route(target_trajectory, 0.25) + self.path_msg.header.stamp = rospy.Time.now() + self.path_msg.header.frame_id = "global" + # clear old waypoints + self.path_msg.poses.clear() + for wp in self.current_trajectory: + pos = PoseStamped() + pos.header.stamp = rospy.Time.now() + pos.header.frame_id = "global" + pos.pose.position.x = wp[0] + pos.pose.position.y = wp[1] + pos.pose.position.z = 704 # needed for visuals + # currently not used therefore zeros + pos.pose.orientation.x = 0 + pos.pose.orientation.y = 0 + pos.pose.orientation.z = 0 + pos.pose.orientation.w = 0 + self.path_msg.poses.append(pos) + + def __current_position_callback(self, data: PoseStamped): + agent = data.pose.position + self.x = agent.x + self.y = agent.y + self.z = agent.z + + # TODO use this to get spawnpoint? necessary? + # use to plot current_position to trajectory for steering test + self.positions.append((self.x, self.y)) + + def __get_heading(self, data: Float32): + self.__current_headings.append(float(data.data)) + + def __get_target_velocity(self, data: Float32): + self.__max_velocities.append(float(data.data)) + + def __get_current_velocity(self, data: CarlaSpeedometer): + self.__current_velocities.append(float(data.speed)) + + def __get_throttle(self, data: Float32): + self.__throttles.append(float(data.data)) + + def __get_purepursuit_steer(self, data: Float32): + r = 1 / (math.pi / 2) + steering_float = float(data.data) * r + self.__purepursuit_steers.append(steering_float) + + def __get_vehicle_steer(self, data: CarlaEgoVehicleControl): + self.__vehicle_steers.append(float(data.steer)) + + def run(self): + """ + Control loop + :return: + """ + self.checkpoint_time = rospy.get_time() + + def loop(timer_event=None): + """ + Publishes different speeds, trajectories ... + depending on the selected TEST_TYPE + """ + # Drive const. velocity on fixed straight steering + if (TEST_TYPE == 0): + self.driveVel = TARGET_VELOCITY_1 + self.pure_pursuit_steer_pub.publish(FIXED_STEERING) + self.velocity_pub.publish(self.driveVel) + + # Drive alternating velocities on fixed straight steering + elif (TEST_TYPE == 1): + if not self.time_set: + self.drive_Vel = TARGET_VELOCITY_1 + self.switch_checkpoint_time = rospy.get_time() + self.switch_time_set = True + if (self.switch_checkpoint_time < rospy.get_time() - 10): + self.switch_checkpoint_time = rospy.get_time() + self.switchVelocity = not self.switchVelocity + if (self.switchVelocity): + self.driveVel = TARGET_VELOCITY_2 + else: + self.driveVel = TARGET_VELOCITY_1 + self.pure_pursuit_steer_pub.publish(FIXED_STEERING) + self.velocity_pub.publish(self.driveVel) + + # drive const. velocity on trajectoy with steering controller + elif (TEST_TYPE == 2): + # Continuously update path and publish it + self.drive_Vel = TARGET_VELOCITY_1 + self.updated_trajectory(self.current_trajectory) + self.trajectory_pub.publish(self.path_msg) + self.velocity_pub.publish(self.driveVel) + + # drive const. velocity on fixed straight steering and + # trigger an emergency brake after 15 secs + elif (TEST_TYPE == 3): + # Continuously update path and publish it + self.drive_Vel = TARGET_VELOCITY_1 + if not self.time_set: + self.checkpoint_time = rospy.get_time() + self.time_set = True + if (self.checkpoint_time < rospy.get_time() - 15.0): + self.checkpoint_time = rospy.get_time() + self.emergency_pub.publish(True) + self.pure_pursuit_steer_pub.publish(FIXED_STEERING) + self.velocity_pub.publish(self.driveVel) + + # --- PRINT TO PLOT --- + # set starttime to when simulation is actually starting to run + # to really get X secs plots every time + if not self.time_set: + self.checkpoint_time = rospy.get_time() + self.time_set = True + # print(">>>>>>>>>>>> TRAJECTORY <<<<<<<<<<<<<<") + # print(self.current_trajectory) + # print(">>>>>>>>>>>> TRAJECTORY <<<<<<<<<<<<<<") + + # Uncomment the prints of the data you want to plot + if (self.checkpoint_time < rospy.get_time() - PRINT_AFTER_TIME): + self.checkpoint_time = rospy.get_time() + print(">>>>>>>>>>>> DATA <<<<<<<<<<<<<<") + # print(self.__max_velocities) + # print(self.__current_velocities) + # print(self.__throttles) + # print(self.__purepursuit_steers) + # print(self.__vehicle_steers) + # print(self.__current_headings) + print(self.positions) + print(">>>>>>>>>>>> DATA <<<<<<<<<<<<<<") + + self.new_timer(self.control_loop_rate, loop) + self.spin() + + +def main(args=None): + """ + main function + :param args: + :return: + """ + + roscomp.init("Acting_Debug_NODE", args=args) + try: + node = Acting_Debug_Node() + node.run() + except KeyboardInterrupt: + pass + finally: + roscomp.shutdown() + + +if __name__ == "__main__": + main() diff --git a/code/acting/src/acting/Acting_DebuggerNode.py b/code/acting/src/acting/OBSOLETE/Acting_Debug_Node_OLD_WITH_STANLEY.py similarity index 93% rename from code/acting/src/acting/Acting_DebuggerNode.py rename to code/acting/src/acting/OBSOLETE/Acting_Debug_Node_OLD_WITH_STANLEY.py index c162c2e7..db2653e2 100755 --- a/code/acting/src/acting/Acting_DebuggerNode.py +++ b/code/acting/src/acting/OBSOLETE/Acting_Debug_Node_OLD_WITH_STANLEY.py @@ -24,9 +24,6 @@ from trajectory_interpolation import interpolate_route -# since this dummy is supposed to test everything ACTING, -# Testers can choose which test to run from changing this Constant - # TEST_TYPE to choose which kind of Test to run: # 0: Test Velocity Controller with constant one velocity # const. velocity = TARGET_VELOCITY_1 @@ -35,7 +32,7 @@ # TURN OFF stanley and PP Controllers in acting.launch! # 1: Test Velocity Controller with changing velocity -# velocity = alternate all 20 secs: TARGET_VELOCITY_1/_HIGH +# velocity = alternate all 20 secs: TARGET_VELOCITY_1/_2 # const. steering = 0 # no trajectory # TURN OFF stanley and PP Controllers in acting.launch! @@ -50,24 +47,20 @@ # const steering = 0 # no trajectory # Triggers emergency break after 15 Seconds -# TODO implement evaluation etc. - -# 4: Test Steering-PID in vehicleController -# TODO TODO -TEST_TYPE = 2 # aka. TT +TEST_TYPE = 2 -FIXED_STEERING: float = 0 # for TestType0 fixed Steering -TARGET_VELOCITY_1: float = 20 # velocity 1 +FIXED_STEERING: float = 0 # if fixed steering needed +TARGET_VELOCITY_1: float = 15 # velocity 1 TARGET_VELOCITY_2: float = 0 # velocity 2 -STEERING_CONTROLLER_USED = 0 # 0 = both ; 1 = PP ; 2 = Stanley +STEERING_CONTROLLER_USED = 1 # 0 = both ; 1 = PP ; 2 = Stanley # 0 = Straight ; 1 = Curve ; 2 = SineWave ; 3 = Overtake -TRAJECTORY_TYPE = 3 +TRAJECTORY_TYPE = 1 -PRINT_AFTER_TIME = 7.0 # How long after Simulationstart to print data +PRINT_AFTER_TIME = 15.0 # How long after Simulationstart to print data -class Acting_Debugger(CompatibleNode): +class Acting_Debug_Node(CompatibleNode): """ Creates a node with testability for all acting components without the need of working/running perception or planning. @@ -78,8 +71,8 @@ def __init__(self): Constructor of the class :return: """ - super(Acting_Debugger, self).__init__('dummy_trajectory_pub') - self.loginfo('Acting_Debugger node started') + super(Acting_Debug_Node, self).__init__('dummy_trajectory_pub') + self.loginfo('Acting_Debug_Node node started') self.role_name = self.get_param('role_name', 'ego_vehicle') self.control_loop_rate = self.get_param('control_loop_rate', 0.05) @@ -178,7 +171,7 @@ def __init__(self): self.__get_vehicle_steer, qos_profile=10) - # Publisher for emergency message TODO: should VC really trigger this? + # Publisher for emergency brake testing self.emergency_pub: Publisher = self.new_publisher( Bool, f"/paf/{self.role_name}/emergency", @@ -188,42 +181,32 @@ def __init__(self): self.current_trajectory = [] self.switchVelocity = False self.driveVel = TARGET_VELOCITY_1 - self.switch_checkpoint_time = rospy.get_time() self.switch_time_set = False - self.checkpoint_time = rospy.get_time() self.time_set = False - self.__current_velocities = [] self.__max_velocities = [] self.__throttles = [] - self.__current_headings = [] - self.__purepursuit_steers = [] self.__stanley_steers = [] self.__vehicle_steers = [] - self.stanley_cross_errors = [] - self.positions = [] + # Generate Trajectory as selected in TRAJECTORY_TYPE self.path_msg = Path() self.path_msg.header.stamp = rospy.Time.now() self.path_msg.header.frame_id = "global" - - # Generate Trajectory as selected in TRAJECTORY_TYPE # Spawncoords at the simulationstart startx = 984.5 starty = -5442.0 - if (TRAJECTORY_TYPE == 0): # Straight trajectory self.current_trajectory = [ (startx, starty), (startx, starty-200) ] - self.updated_trajectory(self.current_trajectory) elif (TRAJECTORY_TYPE == 1): # straight into 90° Curve self.current_trajectory = [ @@ -240,7 +223,6 @@ def __init__(self): (1040.0, -5580.0), (1070.0, -5580.0) ] - self.updated_trajectory(self.current_trajectory) elif (TRAJECTORY_TYPE == 2): # Sinewave Serpentines trajectory # Generate a sine-wave with the global Constants to @@ -267,7 +249,6 @@ def __init__(self): # add a long straight path after the serpentines trajectory_wave.append((startx, starty-200)) self.current_trajectory = trajectory_wave - self.updated_trajectory(self.current_trajectory) elif (TRAJECTORY_TYPE == 3): # 2 Lane Switches self.current_trajectory = [ @@ -311,7 +292,8 @@ def __init__(self): (startx-0.5, starty-100), ] - self.updated_trajectory(self.current_trajectory) + + self.updated_trajectory(self.current_trajectory) def updated_trajectory(self, target_trajectory): """ @@ -330,7 +312,7 @@ def updated_trajectory(self, target_trajectory): pos.header.frame_id = "global" pos.pose.position.x = wp[0] pos.pose.position.y = wp[1] - pos.pose.position.z = 704 # why?? + pos.pose.position.z = 704 # needed for visuals # currently not used therefore zeros pos.pose.orientation.x = 0 pos.pose.orientation.y = 0 @@ -446,6 +428,7 @@ def loop(timer_event=None): elif (STEERING_CONTROLLER_USED == 2): self.controller_selector_pub.publish(2) + # --- PRINT TO PLOT --- # set starttime to when simulation is actually starting to run # to really get 10 secs plots every time if not self.time_set: @@ -481,9 +464,9 @@ def main(args=None): :return: """ - roscomp.init("Acting_Debugger", args=args) + roscomp.init("Acting_Debug_NODE", args=args) try: - node = Acting_Debugger() + node = Acting_Debug_Node() node.run() except KeyboardInterrupt: pass diff --git a/code/acting/src/acting/acc.py b/code/acting/src/acting/OBSOLETE/acc.py similarity index 100% rename from code/acting/src/acting/acc.py rename to code/acting/src/acting/OBSOLETE/acc.py diff --git a/code/acting/src/acting/acc_distance_publisher_dummy.py b/code/acting/src/acting/OBSOLETE/acc_distance_publisher_dummy.py similarity index 100% rename from code/acting/src/acting/acc_distance_publisher_dummy.py rename to code/acting/src/acting/OBSOLETE/acc_distance_publisher_dummy.py diff --git a/code/acting/src/acting/acting_velocity_publisher.py b/code/acting/src/acting/OBSOLETE/acting_velocity_publisher.py similarity index 100% rename from code/acting/src/acting/acting_velocity_publisher.py rename to code/acting/src/acting/OBSOLETE/acting_velocity_publisher.py diff --git a/code/acting/src/acting/stanley_controller.py b/code/acting/src/acting/OBSOLETE/stanley_controller.py similarity index 96% rename from code/acting/src/acting/stanley_controller.py rename to code/acting/src/acting/OBSOLETE/stanley_controller.py index f325ff73..d73c9874 100755 --- a/code/acting/src/acting/stanley_controller.py +++ b/code/acting/src/acting/OBSOLETE/stanley_controller.py @@ -172,8 +172,8 @@ def __calculate_steer(self) -> float: closest_point: PoseStamped = self.__path.poses[closest_point_idx] cross_err = self.__get_cross_err(closest_point.pose.position) # * -1 because it is inverted compared to PurePursuit - steering_angle = -1 * (heading_err + atan((K_CROSSERR * cross_err) - / current_velocity)) + steering_angle = 1 * (heading_err + atan((K_CROSSERR * cross_err) + / current_velocity)) # -> for debugging debug_msg = StanleyDebug() debug_msg.heading = self.__heading @@ -251,8 +251,10 @@ def __get_cross_err(self, pos: Point) -> float: """ dist = self.__dist_to(pos) - x = self.__position[0] - y = self.__position[1] + # +1.4 in Headingdirection = get front axle instead of the cars middle + # heading_deg = self.__heading * 180 / math.pi + x = self.__position[0] # + 1.4 * cos(heading_deg) + y = self.__position[1] # + 1.4 * sin(heading_deg) alpha = 0 if self.__heading is not None: diff --git a/code/acting/src/acting/OBSOLETE/vehicle_controller_OLD_WITH_STANLEY_AND_PID.py b/code/acting/src/acting/OBSOLETE/vehicle_controller_OLD_WITH_STANLEY_AND_PID.py new file mode 100755 index 00000000..c7ae9906 --- /dev/null +++ b/code/acting/src/acting/OBSOLETE/vehicle_controller_OLD_WITH_STANLEY_AND_PID.py @@ -0,0 +1,317 @@ +#!/usr/bin/env python +import math + +import ros_compatibility as roscomp +from ros_compatibility.node import CompatibleNode +from carla_msgs.msg import CarlaEgoVehicleControl, CarlaSpeedometer +from rospy import Publisher, Subscriber +from ros_compatibility.qos import QoSProfile, DurabilityPolicy +from std_msgs.msg import Bool, Float32 +from simple_pid import PID + +PURE_PURSUIT_CONTROLLER: int = 1 +STANLEY_CONTROLLER: int = 2 +STANLEY_CONTROLLER_MIN_V: float = 4.0 # ~14kph + +# PID for PurePursuit +K_P = 4.75 +K_I = 0.0 +K_D = 0.0 +MAX_STEER_ANGLE: float = 0.95 + + +class VehicleController(CompatibleNode): + """ + This node is responsible for collecting all data needed for the + vehicle_control_cmd and sending it. + The node also decides weather to use the stanley or pure pursuit + controller. + If the node receives an emergency msg, it will bring the vehicle to a stop + and send an emergency msg with data = False back, after the velocity of the + vehicle has reached 0. + """ + + def __init__(self): + super(VehicleController, self).__init__('vehicle_controller') + self.loginfo('VehicleController node started') + self.control_loop_rate = self.get_param('control_loop_rate', 0.05) + self.role_name = self.get_param('role_name', 'ego_vehicle') + + # Publisher for Carla Vehicle Control Commands + self.control_publisher: Publisher = self.new_publisher( + CarlaEgoVehicleControl, + f'/carla/{self.role_name}/vehicle_control_cmd', + qos_profile=10 + ) + + # Publisher for Status TODO: Where needed? Why carla? + self.status_pub: Publisher = self.new_publisher( + Bool, + f"/carla/{self.role_name}/status", + qos_profile=QoSProfile( + depth=1, + durability=DurabilityPolicy.TRANSIENT_LOCAL) + ) + + # Publisher for which steering-controller is mainly used + # 1 = PurePursuit and 2 = Stanley + self.controller_pub: Publisher = self.new_publisher( + Float32, + f"/paf/{self.role_name}/controller", + qos_profile=QoSProfile(depth=10, + durability=DurabilityPolicy.TRANSIENT_LOCAL) + ) + + self.emergency_pub: Publisher = self.new_publisher( + Bool, + f"/paf/{self.role_name}/emergency", + qos_profile=QoSProfile(depth=10, + durability=DurabilityPolicy.TRANSIENT_LOCAL) + ) + + self.emergency_sub: Subscriber = self.new_subscription( + Bool, + f"/paf/{self.role_name}/emergency", + self.__emergency_break, + qos_profile=QoSProfile(depth=10, + durability=DurabilityPolicy.TRANSIENT_LOCAL) + ) + + self.velocity_sub: Subscriber = self.new_subscription( + CarlaSpeedometer, + f"/carla/{self.role_name}/Speed", + self.__get_velocity, + qos_profile=1) + + self.throttle_sub: Subscriber = self.new_subscription( + Float32, + f"/paf/{self.role_name}/throttle", + self.__set_throttle, + qos_profile=1) + + self.pure_pursuit_steer_sub: Subscriber = self.new_subscription( + Float32, + f"/paf/{self.role_name}/pure_pursuit_steer", + self.__set_pure_pursuit_steer, + qos_profile=1) + + self.stanley_steer_sub: Subscriber = self.new_subscription( + Float32, + f"/paf/{self.role_name}/stanley_steer", + self.__set_stanley_steer, + qos_profile=1) + + # Testing / Debugging --> + self.brake_sub: Subscriber = self.new_subscription( + Float32, + f"/paf/{self.role_name}/brake", + self.__set_brake, + qos_profile=1) + + self.target_steering_publisher: Publisher = self.new_publisher( + Float32, + f'/paf/{self.role_name}/target_steering_debug', + qos_profile=1) + + self.pidpoint_publisher: Publisher = self.new_publisher( + Float32, + f'/paf/{self.role_name}/pid_point_debug', + qos_profile=1) + + self.controller_selector_sub: Subscriber = self.new_subscription( + Float32, + f'/paf/{self.role_name}/controller_selector_debug', + self.__set_controller, + qos_profile=1) + # <-- Testing / Debugging + + self.__emergency: bool = False + self.__throttle: float = 0.0 + self.__velocity: float = 0.0 + self.__pure_pursuit_steer: float = 0.0 + self.__stanley_steer: float = 0.0 + self.__current_steer: float = 0.0 + + self.__brake: float = 0.0 + + self.controller_testing: bool = False + self.controller_selected_debug: int = 1 + # TODO: check emergency behaviour + + def run(self): + """ + Starts the main loop of the node and send a status msg. + :return: + """ + self.status_pub.publish(True) + self.loginfo('VehicleController node running') + # currently pid for steering is not used, needs fixing + # or maybe deletion since it is not that useful + pid = PID(K_P, K_I, K_D) # PID(0.5, 0.1, 0.1, setpoint=0) + # TODO: TUNE AND FIX? + pid.output_limits = (-MAX_STEER_ANGLE, MAX_STEER_ANGLE) + + def loop(timer_event=None) -> None: + """ + Collects all data received and sends a CarlaEgoVehicleControl msg. + :param timer_event: Timer event from ROS + :return: + """ + if self.__emergency: # emergency is already handled in + # __emergency_break() + return + if (self.controller_testing): + if (self.controller_selected_debug == 2): + p_stanley = 1 + elif (self.controller_selected_debug == 1): + p_stanley = 0 + else: + p_stanley = self.__choose_controller() + + if p_stanley < 0.5: + self.logdebug('Using PURE_PURSUIT_CONTROLLER') + self.controller_pub.publish(float(PURE_PURSUIT_CONTROLLER)) + elif p_stanley >= 0.5: + self.logdebug('Using STANLEY_CONTROLLER') + self.controller_pub.publish(float(STANLEY_CONTROLLER)) + + f_stanley = p_stanley * self.__stanley_steer + f_pure_p = (1 - p_stanley) * self.__pure_pursuit_steer + steer = f_stanley + f_pure_p + + self.target_steering_publisher.publish(steer) # debugging + + message = CarlaEgoVehicleControl() + message.reverse = False + message.throttle = self.__throttle + message.brake = self.__brake + message.hand_brake = False + message.manual_gear_shift = False + pid.setpoint = self.__map_steering(steer) + message.steer = pid(self.__current_steer) + message.gear = 1 + message.header.stamp = roscomp.ros_timestamp(self.get_time(), + from_sec=True) + self.control_publisher.publish(message) + + self.new_timer(self.control_loop_rate, loop) + self.spin() + + def __map_steering(self, steering_angle: float) -> float: + """ + Takes the steering angle calculated by the controller and maps it to + the available steering angle + This is from (left, right): [pi/2 , -pi/2] to [-1, 1] + :param steering_angle: calculated by a controller in [-pi/2 , pi/2] + :return: float for steering in [-1, 1] + """ + steering_float = steering_angle / (math.pi / 2) + self.pidpoint_publisher.publish(steering_float) + return steering_float + + def __emergency_break(self, data) -> None: + """ + Executes emergency stop + :param data: + :return: + """ + if not data.data: # not an emergency + return + if self.__emergency: # emergency was already triggered + return + self.logerr("Emergency breaking engaged") + self.__emergency = True + message = CarlaEgoVehicleControl() + message.throttle = 1 + message.steer = 1 # todo: maybe use 30 degree angle + message.brake = 1 + message.reverse = True + message.hand_brake = True + message.manual_gear_shift = False + message.header.stamp = roscomp.ros_timestamp(self.get_time(), + from_sec=True) + self.control_publisher.publish(message) + + def __get_velocity(self, data: CarlaSpeedometer) -> None: + """ + Ends emergency if vehicle velocity reaches 0. Sends emergency msg + with data = False after stopping. + :param data: + :return: + """ + self.__velocity = data.speed + if not self.__emergency: # nothing to do in this case + return + if data.speed < 0.1: # vehicle has come to a stop + self.__emergency = False + message = CarlaEgoVehicleControl() + message.throttle = 0 + message.steer = 0 + message.brake = 1 + message.reverse = False + message.hand_brake = False + message.manual_gear_shift = False + message.header.stamp = roscomp.ros_timestamp(self.get_time(), + from_sec=True) + self.control_publisher.publish(message) + + self.loginfo("Emergency breaking disengaged " + "(Emergency breaking has been executed successfully)") + for _ in range(7): # publish 7 times just to be safe + self.emergency_pub.publish( + Bool(False)) + + def __set_throttle(self, data): + self.__throttle = data.data + + def __set_brake(self, data): + self.__brake = data.data + + def __set_pure_pursuit_steer(self, data: Float32): + self.__pure_pursuit_steer = data.data + + def __set_stanley_steer(self, data: Float32): + self.__stanley_steer = data.data + + def __set_controller(self, data: Float32): + self.controller_testing = True + self.controller_selected_debug = data.data + + def sigmoid(self, x: float): + """ + Evaluates the sigmoid function s(x) = 1 / (1+e^-25x) + :param x: x + :return: s(x) = 1 / (1+e^-25x) + """ + temp_x = min(-25 * x, 25) + res = 1 / (1 + math.exp(temp_x)) + return res + + def __choose_controller(self) -> float: + """ + Returns the proportion of stanley to use. + Publishes the currently used controller + :return: + """ + res = self.sigmoid(self.__velocity - STANLEY_CONTROLLER_MIN_V) + return res + + +def main(args=None): + """ + Main function starts the node + :param args: + """ + roscomp.init('vehicle_controller', args=args) + + try: + node = VehicleController() + node.run() + except KeyboardInterrupt: + pass + finally: + roscomp.shutdown() + + +if __name__ == '__main__': + main() diff --git a/code/acting/src/acting/pure_pursuit_controller.py b/code/acting/src/acting/pure_pursuit_controller.py index 694c0b1e..dded87b7 100755 --- a/code/acting/src/acting/pure_pursuit_controller.py +++ b/code/acting/src/acting/pure_pursuit_controller.py @@ -15,12 +15,16 @@ from helper_functions import vector_angle from trajectory_interpolation import points_to_vector -# Tuneable Values +# Tuneable Values for PurePursuit-Algorithm K_LAD = 0.85 MIN_LA_DISTANCE = 2 MAX_LA_DISTANCE = 25 -# Constants -L_VEHICLE = 2.85 # wheelbase +# Tuneable Factor before Publishing +# "-" because it is inverted to the steering carla expects +# "4.75" proved to be important for a good steering (see tuning-documentation) +K_PUB = (-4.75) +# Constant: wheelbase of car +L_VEHICLE = 2.85 class PurePursuitController(CompatibleNode): @@ -61,22 +65,16 @@ def __init__(self): f"/paf/{self.role_name}/pure_pursuit_steer", qos_profile=1) - self.debug_publisher: Publisher = self.new_publisher( + self.debug_msg_pub: Publisher = self.new_publisher( Debug, f"/paf/{self.role_name}/pure_p_debug", qos_profile=1) self.__position: (float, float) = None # x, y - self.__last_pos: (float, float) = None self.__path: Path = None self.__heading: float = None self.__velocity: float = None self.__tp_idx: int = 0 # target waypoint index - # error when there are no targets - - self.time_set = False - self.checker = False - self.checkpoint_time = -1 def run(self): """ @@ -141,16 +139,15 @@ def __calculate_steer(self) -> float: # Get the error between current heading and target heading alpha = target_vector_heading - self.__heading # https://thomasfermi.github.io/Algorithms-for-Automated-Driving/Control/PurePursuit.html - steering_angle = atan((2 * L_VEHICLE * sin(alpha)) / look_ahead_dist) - + steering_angle = K_PUB * steering_angle # for debugging -> debug_msg = Debug() debug_msg.heading = self.__heading debug_msg.target_heading = target_vector_heading debug_msg.l_distance = look_ahead_dist debug_msg.steering_angle = steering_angle - self.debug_publisher.publish(debug_msg) + self.debug_msg_pub.publish(debug_msg) # <- return steering_angle @@ -170,7 +167,6 @@ def __set_position(self, data: PoseStamped, min_diff=0.001): y0 = data.pose.position.y self.__position = (x0, y0) return - # check if the new position is valid dist = self.__dist_to(data.pose.position) if dist < min_diff: @@ -181,10 +177,6 @@ def __set_position(self, data: PoseStamped, min_diff=0.001): f"as dist ({round(dist, 3)}) to current pos " f"< min_diff ({round(min_diff, 3)})") return - # TODO: why save the old position if it is never used again? - old_x = self.__position[0] - old_y = self.__position[1] - self.__last_pos = (old_x, old_y) new_x = data.pose.position.x new_y = data.pose.position.y self.__position = (new_x, new_y) @@ -213,7 +205,6 @@ def __get_target_point_index(self, ld: float) -> int: :param ld: look ahead distance :return: """ - # if path has less than 2 poses, break if len(self.__path.poses) < 2: return -1 diff --git a/code/acting/src/acting/vehicle_controller.py b/code/acting/src/acting/vehicle_controller.py index 338a23e3..4ea8f4a0 100755 --- a/code/acting/src/acting/vehicle_controller.py +++ b/code/acting/src/acting/vehicle_controller.py @@ -7,13 +7,6 @@ from rospy import Publisher, Subscriber from ros_compatibility.qos import QoSProfile, DurabilityPolicy from std_msgs.msg import Bool, Float32 -from simple_pid import PID - -PURE_PURSUIT_CONTROLLER: int = 1 -STANLEY_CONTROLLER: int = 2 -STANLEY_CONTROLLER_MIN_V: float = 4.0 # ~14kph - -MAX_STEER_ANGLE: float = 0.75 class VehicleController(CompatibleNode): @@ -58,14 +51,13 @@ def __init__(self): durability=DurabilityPolicy.TRANSIENT_LOCAL) ) - # Publisher for emergency message TODO: should VC really trigger this? self.emergency_pub: Publisher = self.new_publisher( Bool, f"/paf/{self.role_name}/emergency", qos_profile=QoSProfile(depth=10, durability=DurabilityPolicy.TRANSIENT_LOCAL) ) - # Subscriber for emergency TODO: who can trigger this, what happens? + self.emergency_sub: Subscriber = self.new_subscription( Bool, f"/paf/{self.role_name}/emergency", @@ -86,54 +78,23 @@ def __init__(self): self.__set_throttle, qos_profile=1) - self.pure_pursuit_steer_sub: Subscriber = self.new_subscription( - Float32, - f"/paf/{self.role_name}/pure_pursuit_steer", - self.__set_pure_pursuit_steer, - qos_profile=1) - - self.stanley_steer_sub: Subscriber = self.new_subscription( - Float32, - f"/paf/{self.role_name}/stanley_steer", - self.__set_stanley_steer, - qos_profile=1) - - # Testing / Debugging --> self.brake_sub: Subscriber = self.new_subscription( Float32, f"/paf/{self.role_name}/brake", self.__set_brake, qos_profile=1) - self.target_steering_publisher: Publisher = self.new_publisher( - Float32, - f'/paf/{self.role_name}/target_steering_debug', - qos_profile=1) - - self.pidpoint_publisher: Publisher = self.new_publisher( - Float32, - f'/paf/{self.role_name}/pid_point_debug', - qos_profile=1) - - self.controller_selector_sub: Subscriber = self.new_subscription( + self.pure_pursuit_steer_sub: Subscriber = self.new_subscription( Float32, - f'/paf/{self.role_name}/controller_selector_debug', - self.__set_controller, + f"/paf/{self.role_name}/pure_pursuit_steer", + self.__set_pure_pursuit_steer, qos_profile=1) - # <-- Testing / Debugging self.__emergency: bool = False - self.__throttle: float = 0.0 self.__velocity: float = 0.0 - self.__pure_pursuit_steer: float = 0.0 - self.__stanley_steer: float = 0.0 - self.__current_steer: float = 0.0 - self.__brake: float = 0.0 - - self.controller_testing: bool = False - self.controller_selected_debug: int = 1 - # TODO: check emergency behaviour + self.__throttle: float = 0.0 + self.__steer: float = 0.0 def run(self): """ @@ -142,11 +103,6 @@ def run(self): """ self.status_pub.publish(True) self.loginfo('VehicleController node running') - # currently pid for steering is not used, needs fixing - # or maybe deletion since it is not that useful - pid = PID(0.5, 0.001, 0) # PID(0.5, 0.1, 0.1, setpoint=0) - # TODO: TUNE AND FIX? - pid.output_limits = (-MAX_STEER_ANGLE, MAX_STEER_ANGLE) def loop(timer_event=None) -> None: """ @@ -154,50 +110,17 @@ def loop(timer_event=None) -> None: :param timer_event: Timer event from ROS :return: """ - if self.__emergency: # emergency is already handled in - # __emergency_break() + if self.__emergency: + # emergency is already handled in __emergency_break() return - if (self.controller_testing): - if (self.controller_selected_debug == 2): - p_stanley = 1 - elif (self.controller_selected_debug == 1): - p_stanley = 0 - else: - p_stanley = self.__choose_controller() - - if p_stanley < 0.5: - self.logdebug('Using PURE_PURSUIT_CONTROLLER') - self.controller_pub.publish(float(PURE_PURSUIT_CONTROLLER)) - elif p_stanley >= 0.5: - self.logdebug('Using STANLEY_CONTROLLER') - self.controller_pub.publish(float(STANLEY_CONTROLLER)) - - f_stanley = p_stanley * self.__stanley_steer - f_pure_p = (1 - p_stanley) * self.__pure_pursuit_steer - steer = f_stanley + f_pure_p - - # -- Hardcode steering to only use Stanley -- - # ---------------- - steer = self.__pure_pursuit_steer - # steer = self.__stanley_steer - # steer = 0.5 * self.__stanley_steer - # + 0.5 * self.__pure_pursuit_steer - # ---------------- - # -- Hardcode steering to only use Stanley -- - - self.target_steering_publisher.publish(steer) # debugging - message = CarlaEgoVehicleControl() message.reverse = False - message.throttle = self.__throttle - message.brake = self.__brake message.hand_brake = False message.manual_gear_shift = False - message.steer = self.__map_steering(steer) - # Original Code: - # pid.setpoint = self.__map_steering(steer) - # message.steer = pid(self.__current_steer) message.gear = 1 + message.throttle = self.__throttle + message.brake = self.__brake + message.steer = self.__steer message.header.stamp = roscomp.ros_timestamp(self.get_time(), from_sec=True) self.control_publisher.publish(message) @@ -205,19 +128,6 @@ def loop(timer_event=None) -> None: self.new_timer(self.control_loop_rate, loop) self.spin() - def __map_steering(self, steering_angle: float) -> float: - """ - Takes the steering angle calculated by the controller and maps it to - the available steering angle - This is from (left, right): [pi/2 , -pi/2] to [-1, 1] - :param steering_angle: calculated by a controller in [-pi/2 , pi/2] - :return: float for steering in [-1, 1] - """ - r = - 1 / (math.pi / 2) # -1 to invert for carla steering - steering_float = steering_angle * r # No Tuning needed * tune_k - self.pidpoint_publisher.publish(steering_float) # TODO needed? - return steering_float - def __emergency_break(self, data) -> None: """ Executes emergency stop @@ -251,7 +161,7 @@ def __get_velocity(self, data: CarlaSpeedometer) -> None: self.__velocity = data.speed if not self.__emergency: # nothing to do in this case return - if data.speed < 0.1: # vehicle has come to a stop + if self.__velocity < 0.1: # vehicle has come to a stop self.__emergency = False message = CarlaEgoVehicleControl() message.throttle = 0 @@ -277,33 +187,8 @@ def __set_brake(self, data): self.__brake = data.data def __set_pure_pursuit_steer(self, data: Float32): - self.__pure_pursuit_steer = data.data - - def __set_stanley_steer(self, data: Float32): - self.__stanley_steer = data.data - - def __set_controller(self, data: Float32): - self.controller_testing = True - self.controller_selected_debug = data.data - - def sigmoid(self, x: float): - """ - Evaluates the sigmoid function s(x) = 1 / (1+e^-25x) - :param x: x - :return: s(x) = 1 / (1+e^-25x) - """ - temp_x = min(-25 * x, 25) - res = 1 / (1 + math.exp(temp_x)) - return res - - def __choose_controller(self) -> float: - """ - Returns the proportion of stanley to use. - Publishes the currently used controller - :return: - """ - res = self.sigmoid(self.__velocity - STANLEY_CONTROLLER_MIN_V) - return res + r = (math.pi / 2) # convert from RAD to [-1;1] + self.__steer = (data.data / r) def main(args=None): From 7a4241d882701839eaa6c3eca732103a63b18a01 Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Sun, 10 Mar 2024 14:37:18 +0100 Subject: [PATCH 32/43] 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 33/43] 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 34/43] 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 @@ - + diff --git a/code/agent/launch/dev.launch b/code/agent/launch/dev.launch index cc1f148d..ddfae533 100644 --- a/code/agent/launch/dev.launch +++ b/code/agent/launch/dev.launch @@ -15,7 +15,7 @@ - + From d5ba47228951dcd4c5a65990e2f84adb2df94d7d Mon Sep 17 00:00:00 2001 From: ll7 Date: Tue, 12 Mar 2024 17:34:35 +0100 Subject: [PATCH 40/43] feat: add a b5 run_dev task --- build/Taskfile | 5 ++ build/docker-compose.dev.yaml | 90 +++++++++++++++++++++++++++++++++++ 2 files changed, 95 insertions(+) create mode 100644 build/docker-compose.dev.yaml diff --git a/build/Taskfile b/build/Taskfile index 9ad466af..b152bc75 100644 --- a/build/Taskfile +++ b/build/Taskfile @@ -11,6 +11,11 @@ task:run() { docker:compose up "$@" } +task:run_dev() { + xhost +local:docker + docker:compose -f docker-compose.dev.yaml up +} + task:restart() { container="$1" docker:compose restart "${container:-agent}" diff --git a/build/docker-compose.dev.yaml b/build/docker-compose.dev.yaml new file mode 100644 index 00000000..6ba17e3d --- /dev/null +++ b/build/docker-compose.dev.yaml @@ -0,0 +1,90 @@ +version: "3" + +services: + flake8: + image: alpine/flake8 + command: . + volumes: + - ../:/apps + + comlipy: + build: docker/comlipy + command: . + volumes: + - ../:/apps + + mdlint: + image: peterdavehello/markdownlint:0.32.2 + command: markdownlint . + volumes: + - ../:/md + + # based on https://github.com/ll7/paf21-1/blob/master/scenarios/docker-carla-sim-compose.yml + carla-simulator: + command: /bin/bash CarlaUE4.sh -quality-level=Epic -world-port=2000 -resx=800 -resy=600 -nosound -carla-settings="/home/carla/CarlaUE4/Config/CustomCarlaSettings.ini" + image: ghcr.io/una-auxme/paf23:leaderboard-2.0 + init: true + deploy: + resources: + limits: + memory: 16G + expose: + - 2000 + - 2001 + - 2002 + environment: + - XDG_RUNTIME_DIR=/tmp/runtime-carla + networks: + - carla + volumes: + - /tmp/.X11-unix:/tmp/.X11-unix + - ./CustomCarlaSettings.ini:/home/carla/CarlaUE4/Config/CustomCarlaSettings.ini + + roscore: + image: ros:noetic + command: roscore + environment: + - ROS_MASTER_URI=http://roscore:11311 + - ROS_HOSTNAME=roscore + expose: + - 11311 + networks: + - ros + + agent: + build: + dockerfile: build/docker/agent/Dockerfile + args: + - USER_UID=${DOCKER_HOST_UNIX_UID:-1000} + - USER_GID=${DOCKER_HOST_UNIX_GID:-1000} + context: ../ + init: true + 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" + + logging: + driver: "local" + environment: + - DISPLAY + - ROS_MASTER_URI=http://roscore:11311 + - CARLA_SIM_HOST=carla-simulator + - ROS_HOSTNAME=agent + - XDG_RUNTIME_DIR=/tmp/runtime-carla + volumes: + - /tmp/.X11-unix:/tmp/.X11-unix + # if you change the volume here also change the copy command + # in ``build/docker/build/Dockerfile + - ../:/workspace + # mount git config for dvc + - ../.gitconfig:/home/carla/.gitconfig + - ../:/workspace/ + networks: + - carla + - ros + +networks: + carla: + ros: From c852c22ccaafa34f27d2d34ce74a8f668d58b7d2 Mon Sep 17 00:00:00 2001 From: ll7 Date: Tue, 12 Mar 2024 17:41:42 +0100 Subject: [PATCH 41/43] fix: revert lidar rotation --- code/agent/src/agent/agent.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/code/agent/src/agent/agent.py b/code/agent/src/agent/agent.py index 2c2b6130..22b053c7 100755 --- a/code/agent/src/agent/agent.py +++ b/code/agent/src/agent/agent.py @@ -55,7 +55,7 @@ def sensors(self): 'type': 'sensor.lidar.ray_cast', 'id': 'LIDAR', 'x': 0.0, 'y': 0.0, 'z': 1.70, - 'roll': 0.0, 'pitch': 0.0, 'yaw': math.radians(90.0) + 'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0 }, { 'type': 'sensor.other.radar', From 6826472d136274daa8c4a4a2b1ccacced8400c68 Mon Sep 17 00:00:00 2001 From: hellschwalex Date: Thu, 14 Mar 2024 14:11:26 +0100 Subject: [PATCH 42/43] feat: hotfixing steering for leaderboard --- code/acting/launch/acting.launch | 4 ++-- .../src/acting/pure_pursuit_controller.py | 7 +++--- .../{OBSOLETE => }/stanley_controller.py | 9 +++----- code/acting/src/acting/vehicle_controller.py | 22 ++++++++++++++++--- 4 files changed, 28 insertions(+), 14 deletions(-) rename code/acting/src/acting/{OBSOLETE => }/stanley_controller.py (97%) diff --git a/code/acting/launch/acting.launch b/code/acting/launch/acting.launch index fd0aabb3..ba12415c 100644 --- a/code/acting/launch/acting.launch +++ b/code/acting/launch/acting.launch @@ -31,12 +31,12 @@ --> - - + diff --git a/code/acting/src/acting/pure_pursuit_controller.py b/code/acting/src/acting/pure_pursuit_controller.py index dded87b7..e53801b9 100755 --- a/code/acting/src/acting/pure_pursuit_controller.py +++ b/code/acting/src/acting/pure_pursuit_controller.py @@ -20,9 +20,10 @@ MIN_LA_DISTANCE = 2 MAX_LA_DISTANCE = 25 # Tuneable Factor before Publishing -# "-" because it is inverted to the steering carla expects -# "4.75" proved to be important for a good steering (see tuning-documentation) -K_PUB = (-4.75) +# "-1" because it is inverted to the steering carla expects +# "4.75" proved to be important for a good steering +# ONLY IN DEV.LAUNCH? (see documentation) +K_PUB = -0.85 # (-4.75) # Constant: wheelbase of car L_VEHICLE = 2.85 diff --git a/code/acting/src/acting/OBSOLETE/stanley_controller.py b/code/acting/src/acting/stanley_controller.py similarity index 97% rename from code/acting/src/acting/OBSOLETE/stanley_controller.py rename to code/acting/src/acting/stanley_controller.py index d73c9874..34567787 100755 --- a/code/acting/src/acting/OBSOLETE/stanley_controller.py +++ b/code/acting/src/acting/stanley_controller.py @@ -15,7 +15,7 @@ from helper_functions import vector_angle from trajectory_interpolation import points_to_vector -K_CROSSERR = 1.24 +K_CROSSERR = 0.4 # 1.24 class StanleyController(CompatibleNode): @@ -250,11 +250,8 @@ def __get_cross_err(self, pos: Point) -> float: :return: """ dist = self.__dist_to(pos) - - # +1.4 in Headingdirection = get front axle instead of the cars middle - # heading_deg = self.__heading * 180 / math.pi - x = self.__position[0] # + 1.4 * cos(heading_deg) - y = self.__position[1] # + 1.4 * sin(heading_deg) + x = self.__position[0] + y = self.__position[1] alpha = 0 if self.__heading is not None: diff --git a/code/acting/src/acting/vehicle_controller.py b/code/acting/src/acting/vehicle_controller.py index 5da73ad8..36900c00 100755 --- a/code/acting/src/acting/vehicle_controller.py +++ b/code/acting/src/acting/vehicle_controller.py @@ -90,11 +90,18 @@ def __init__(self): self.__set_pure_pursuit_steer, qos_profile=1) + self.stanley_sub: Subscriber = self.new_subscription( + Float32, + f"/paf/{self.role_name}/stanley_steer", + self.__set_stanley_steer, + qos_profile=1) + self.__emergency: bool = False self.__velocity: float = 0.0 self.__brake: float = 0.0 self.__throttle: float = 0.0 - self.__steer: float = 0.0 + self._p_steer: float = 0.0 + self._s_steer: float = 0.0 def run(self): """ @@ -116,6 +123,11 @@ def loop(timer_event=None) -> None: # emergency is already handled in __emergency_brake() self.__emergency_brake(True) return + + if self.__velocity > 5: + steer = self._s_steer + else: + steer = self._p_steer message = CarlaEgoVehicleControl() message.reverse = False message.hand_brake = False @@ -123,7 +135,7 @@ def loop(timer_event=None) -> None: message.gear = 1 message.throttle = self.__throttle message.brake = self.__brake - message.steer = self.__steer + message.steer = steer message.header.stamp = roscomp.ros_timestamp(self.get_time(), from_sec=True) self.control_publisher.publish(message) @@ -203,7 +215,11 @@ def __set_brake(self, data): def __set_pure_pursuit_steer(self, data: Float32): r = (math.pi / 2) # convert from RAD to [-1;1] - self.__steer = (data.data / r) + self._p_steer = (data.data / r) + + def __set_stanley_steer(self, data: Float32): + r = (math.pi / 2) # convert from RAD to [-1;1] + self._s_steer = (data.data / r) def main(args=None): From fa298348aaac3947c5861ac7ad56c7ea22ff10a1 Mon Sep 17 00:00:00 2001 From: MaxJa4 <74194322+MaxJa4@users.noreply.github.com> Date: Mon, 18 Mar 2024 12:45:48 +0100 Subject: [PATCH 43/43] feat: Filter out side-facing traffic lights by HSV and shape (#209) --- code/perception/src/traffic_light_node.py | 49 ++++++++++++++++++++++- 1 file changed, 48 insertions(+), 1 deletion(-) diff --git a/code/perception/src/traffic_light_node.py b/code/perception/src/traffic_light_node.py index 00034bab..66b250ee 100755 --- a/code/perception/src/traffic_light_node.py +++ b/code/perception/src/traffic_light_node.py @@ -11,6 +11,8 @@ from cv_bridge import CvBridge from traffic_light_detection.src.traffic_light_detection.traffic_light_inference \ import TrafficLightInference # noqa: E501 +import cv2 +import numpy as np class TrafficLightNode(CompatibleNode): @@ -58,12 +60,17 @@ def auto_invalidate_state(self): self.last_info_time = None def handle_camera_image(self, image): - result, data = self.classifier(self.bridge.imgmsg_to_cv2(image)) + cv2_image = self.bridge.imgmsg_to_cv2(image) + rgb_image = cv2.cvtColor(cv2_image, cv2.COLOR_BGR2RGB) + result, data = self.classifier(cv2_image) if data[0][0] > 1e-15 and data[0][3] > 1e-15 or \ data[0][0] > 1e-10 or data[0][3] > 1e-10: return # too uncertain, may not be a traffic light + if not is_front(rgb_image): + return # not a front facing traffic light + state = result if result in [1, 2, 4] else 0 if self.last_state == state: # 1: Green, 2: Red, 4: Yellow, 0: Unknown @@ -81,6 +88,46 @@ def run(self): self.spin() +def get_light_mask(image): + hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV) + + # Define lower and upper bounds for the hue, saturation, and value + # - Red and Yellow combined since they are so close in the color spectrum + lower_red_yellow = np.array([0, 75, 100]) + upper_red_yellow = np.array([40, 255, 255]) + lower_green = np.array([40, 200, 200]) + upper_green = np.array([80, 255, 255]) + + # Mask where the pixels within the bounds are white, otherwise black + m1 = cv2.inRange(hsv, lower_red_yellow, upper_red_yellow) + m2 = cv2.inRange(hsv, lower_green, upper_green) + mask = cv2.bitwise_or(m1, m2) + + return mask + + +def is_front(image): + mask = get_light_mask(image) + + # Find contours in the thresholded image, use only the largest one + contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, + cv2.CHAIN_APPROX_SIMPLE) + contours = sorted(contours, key=cv2.contourArea, reverse=True)[:1] + contour = contours[0] if contours else None + + if contour is None: + return False + + _, _, width, height = cv2.boundingRect(contour) + aspect_ratio = width / height + + # If aspect ratio is within range of a square (therefore a circle) + if 0.75 <= aspect_ratio <= 1.3: + return True + else: + return False + + if __name__ == "__main__": roscomp.init("TrafficLightNode") node = TrafficLightNode("TrafficLightNode")