From 22306536e50ee0157a48cdd360de49e0a15ad24b Mon Sep 17 00:00:00 2001 From: JuliusMiller Date: Tue, 19 Mar 2024 15:17:33 +0100 Subject: [PATCH 1/4] fix: adjust decision making for new traffic light --- build/docker-compose.yml | 4 +- .../src/global_plan_distance_publisher.py | 6 --- code/perception/src/vision_node.py | 23 ++++++------ code/planning/launch/planning.launch | 4 +- .../behavior_agent/behaviours/intersection.py | 14 +++++-- .../behaviours/topics2blackboard.py | 6 ++- .../src/global_planner/dev_global_route.py | 8 ++-- .../src/global_planner/global_planner.py | 8 +--- .../src/local_planner/motion_planning.py | 37 +++++++++++++------ 9 files changed, 60 insertions(+), 50 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/perception/src/global_plan_distance_publisher.py b/code/perception/src/global_plan_distance_publisher.py index 40da367c..4c53ca71 100755 --- a/code/perception/src/global_plan_distance_publisher.py +++ b/code/perception/src/global_plan_distance_publisher.py @@ -44,17 +44,11 @@ def __init__(self): self.update_position, qos_profile=1) - # Change comment for dev_launch self.global_plan_subscriber = self.new_subscription( CarlaRoute, "/carla/" + self.role_name + "/global_plan", self.update_global_route, qos_profile=1) - # self.global_plan_subscriber = self.new_subscription( - # CarlaRoute, - # "/paf/" + self.role_name + "/global_plan", - # self.update_global_route, - # qos_profile=1) self.waypoint_publisher = self.new_publisher( Waypoint, diff --git a/code/perception/src/vision_node.py b/code/perception/src/vision_node.py index c9270924..d69efbcf 100755 --- a/code/perception/src/vision_node.py +++ b/code/perception/src/vision_node.py @@ -20,7 +20,7 @@ import numpy as np from ultralytics import NAS, YOLO, RTDETR, SAM, FastSAM import asyncio -import rospy +# import rospy """ VisionNode: @@ -218,15 +218,16 @@ def handle_camera_image(self, image): 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] - if side == "Center": - self.publisher_center.publish(img_msg) - if side == "Back": - self.publisher_back.publish(img_msg) - if side == "Left": - self.publisher_left.publish(img_msg) - if side == "Right": - self.publisher_right.publish(img_msg) + self.publisher_center.publish(img_msg) + # side = rospy.resolve_name(img_msg.header.frame_id).split('/')[2] + # if side == "Center": + # self.publisher_center.publish(img_msg) + # if side == "Back": + # self.publisher_back.publish(img_msg) + # if side == "Left": + # self.publisher_left.publish(img_msg) + # if side == "Right": + # self.publisher_right.publish(img_msg) # locals().clear() # print(f"Published Image on Side: {side}") @@ -414,7 +415,7 @@ def predict_ultralytics(self, image): # return output[0].plot() - def process_traffic_lights(self, prediction, cv_image, image_header): + async def process_traffic_lights(self, prediction, cv_image, image_header): indices = (prediction.boxes.cls == 9).nonzero().squeeze().cpu().numpy() indices = np.asarray([indices]) if indices.size == 1 else indices diff --git a/code/planning/launch/planning.launch b/code/planning/launch/planning.launch index 7437344e..354b8d65 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/intersection.py b/code/planning/src/behavior_agent/behaviours/intersection.py index 7e152726..c780849e 100755 --- a/code/planning/src/behavior_agent/behaviours/intersection.py +++ b/code/planning/src/behavior_agent/behaviours/intersection.py @@ -102,9 +102,13 @@ def update(self): # Update Light Info light_status_msg = self.blackboard.get( "/paf/hero/Center/traffic_light_state") + light_distance_y_msg = self.blackboard.get( + "/paf/hero/Center/traffic_light_y_distance") if light_status_msg is not None: self.traffic_light_status = get_color(light_status_msg.state) self.traffic_light_detected = True + if light_distance_y_msg is not None: + self.traffic_light_distance = light_distance_y_msg.data # Update stopline Info _dis = self.blackboard.get("/paf/hero/waypoint_distance") @@ -127,6 +131,7 @@ def update(self): self.virtual_stopline_distance = 0.0 rospy.loginfo(f"Stopline distance: {self.virtual_stopline_distance}") + rospy.loginfo(f"y-distance: {self.traffic_light_distance}") target_distance = 5.0 # stop when there is no or red/yellow traffic light or a stop sign is # detected @@ -151,12 +156,14 @@ def update(self): else: rospy.logwarn("no speedometer connected") return py_trees.common.Status.RUNNING - if self.virtual_stopline_distance > target_distance: + if (self.virtual_stopline_distance > target_distance) and \ + (self.traffic_light_distance > 150): # too far print("still approaching") return py_trees.common.Status.RUNNING elif speed < convert_to_ms(2.0) and \ - self.virtual_stopline_distance < target_distance: + ((self.virtual_stopline_distance < target_distance) or + (self.traffic_light_distance < 150)): # stopped print("stopped") return py_trees.common.Status.SUCCESS @@ -265,8 +272,7 @@ def update(self): light_status_msg = self.blackboard.get( "/paf/hero/Center/traffic_light_state") - lidar_data = self.blackboard.get("/carla/hero/LIDAR_range") - + lidar_data = None intersection_clear = True if lidar_data is not None: # if distance smaller than 10m, intersection is blocked diff --git a/code/planning/src/behavior_agent/behaviours/topics2blackboard.py b/code/planning/src/behavior_agent/behaviours/topics2blackboard.py index d26a717d..40853edf 100755 --- a/code/planning/src/behavior_agent/behaviours/topics2blackboard.py +++ b/code/planning/src/behavior_agent/behaviours/topics2blackboard.py @@ -4,7 +4,7 @@ import py_trees import py_trees_ros -from std_msgs.msg import Float32, Bool, Float32MultiArray +from std_msgs.msg import Float32, Bool, Float32MultiArray, Int16 from carla_msgs.msg import CarlaSpeedometer from geometry_msgs.msg import PoseStamped @@ -36,6 +36,10 @@ def create_node(role_name): f"/paf/{role_name}/Center/traffic_light_state", 'msg': TrafficLightState, 'clearing-policy': py_trees.common.ClearingPolicy.NEVER}, + {'name': + f"/paf/{role_name}/Center/traffic_light_y_distance", + 'msg': Int16, + 'clearing-policy': py_trees.common.ClearingPolicy.NEVER}, {'name': f"/paf/{role_name}/max_velocity", 'msg': Float32, 'clearing-policy': py_trees.common.ClearingPolicy.NEVER}, {'name': f"/paf/{role_name}/speed_limit", 'msg': Float32, diff --git a/code/planning/src/global_planner/dev_global_route.py b/code/planning/src/global_planner/dev_global_route.py index 1857293c..1d455ac7 100755 --- a/code/planning/src/global_planner/dev_global_route.py +++ b/code/planning/src/global_planner/dev_global_route.py @@ -14,13 +14,11 @@ """ This node is currently used for importing a CarlaRoute to dev.launch. -For this you need to follow 4 steps: +For this you need to follow 3 steps: 1. In plannning.launch: Uncomment this node and change the .txt file if needed -2. In global_planner.py: Change the subscriber of carla/hero/global_plan to paf -3. In gobal_planner.py: At the end of the init uncomment the +2. In gobal_planner.py: At the end of the init uncomment the self.dev_load_world_info() -4. In global_plan_distance_publisher.py: - Change the subscriber of carla/hero/global_plan to paf +3. In docker-compose.yml switch to the dev environment """ diff --git a/code/planning/src/global_planner/global_planner.py b/code/planning/src/global_planner/global_planner.py index 23442530..361ab5b5 100755 --- a/code/planning/src/global_planner/global_planner.py +++ b/code/planning/src/global_planner/global_planner.py @@ -54,17 +54,11 @@ def __init__(self): callback=self.world_info_callback, qos_profile=10) - # 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', callback=self.global_route_callback, qos_profile=10) - # self.global_plan_sub = self.new_subscription( - # msg_type=CarlaRoute, - # topic='/paf/' + self.role_name + '/global_plan', - # callback=self.global_route_callback, - # qos_profile=10) self.current_pos_sub = self.new_subscription( msg_type=PoseStamped, @@ -84,7 +78,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/motion_planning.py b/code/planning/src/local_planner/motion_planning.py index 10f57135..22b7a62b 100755 --- a/code/planning/src/local_planner/motion_planning.py +++ b/code/planning/src/local_planner/motion_planning.py @@ -5,7 +5,7 @@ from ros_compatibility.node import CompatibleNode from rospy import Publisher, Subscriber -from std_msgs.msg import String, Float32, Bool, Float32MultiArray +from std_msgs.msg import String, Float32, Bool, Float32MultiArray, Int16 from nav_msgs.msg import Path from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion from carla_msgs.msg import CarlaSpeedometer @@ -54,6 +54,7 @@ def __init__(self): self.__corners = None self.__in_corner = False self.calculated = False + self.traffic_light_y_distance = np.inf # Subscriber self.test_sub = self.new_subscription( Float32, @@ -114,12 +115,18 @@ def __init__(self): self.__set_change_point, qos_profile=1) - self.change_point_sub: Subscriber = self.new_subscription( + self.coll_point_sub: Subscriber = self.new_subscription( Float32MultiArray, f"/paf/{self.role_name}/collision", self.__set_collision_point, qos_profile=1) + self.traffic_y_sub: Subscriber = self.new_subscription( + Int16, + f"/paf/{self.role_name}/Center/traffic_light_y_distance", + self.__set_traffic_y_distance, + qos_profile=1) + # Publisher self.traj_pub: Publisher = self.new_publisher( msg_type=Path, @@ -184,6 +191,10 @@ def __set_current_pos(self, data: PoseStamped): data.pose.position.y, data.pose.position.z]) + def __set_traffic_y_distance(self, data): + if data is not None: + self.traffic_light_y_distance = data.data + def change_trajectory(self, distance_obj): """update trajectory for overtaking and convert it to a new Path message @@ -477,14 +488,14 @@ def __get_speed_cruise(self) -> float: def __calc_speed_to_stop_intersection(self) -> float: target_distance = 5.0 - virtual_stopline_distance = self.__calc_virtual_stopline() + stopline = self.__calc_virtual_stopline() # calculate speed needed for stopping + self.logerr(f"MP: {stopline}") v_stop = max(convert_to_ms(10.), - convert_to_ms((virtual_stopline_distance / 30) - * 50)) + convert_to_ms(stopline / 0.8)) if v_stop > bs.int_app_init.speed: v_stop = bs.int_app_init.speed - if virtual_stopline_distance < target_distance: + if stopline < target_distance: v_stop = 0.0 return v_stop @@ -493,8 +504,7 @@ def __calc_speed_to_stop_lanechange(self) -> float: stopline = self.__calc_virtual_change_point() v_stop = max(convert_to_ms(10.), - convert_to_ms((stopline / 30) - * 50)) + convert_to_ms(stopline / 0.8)) if v_stop > bs.lc_app_init.speed: v_stop = bs.lc_app_init.speed if stopline < 5.0: @@ -503,10 +513,8 @@ def __calc_speed_to_stop_lanechange(self) -> float: def __calc_speed_to_stop_overtake(self) -> float: stopline = self.__calc_virtual_overtake() - v_stop = max(convert_to_ms(10.), - convert_to_ms((stopline / 30) - * 50)) + convert_to_ms(stopline / 0.8)) if stopline < 6.0: v_stop = 0.0 @@ -521,7 +529,12 @@ def __calc_virtual_change_point(self) -> float: def __calc_virtual_stopline(self) -> float: if self.__stopline[0] != np.inf and self.__stopline[1]: - return self.__stopline[0] + if self.traffic_light_y_distance < 200: + return 10 + elif self.traffic_light_y_distance < 150: + return 0.0 + else: + return self.__stopline[0] else: return 0.0 From 312ad3b63baaf76ff10ac541868c196d3a4ad12e Mon Sep 17 00:00:00 2001 From: JuliusMiller Date: Tue, 19 Mar 2024 15:51:26 +0100 Subject: [PATCH 2/4] fix: Adjust stopdistance --- code/planning/src/local_planner/motion_planning.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/code/planning/src/local_planner/motion_planning.py b/code/planning/src/local_planner/motion_planning.py index 22b7a62b..82b777d0 100755 --- a/code/planning/src/local_planner/motion_planning.py +++ b/code/planning/src/local_planner/motion_planning.py @@ -529,12 +529,13 @@ def __calc_virtual_change_point(self) -> float: def __calc_virtual_stopline(self) -> float: if self.__stopline[0] != np.inf and self.__stopline[1]: - if self.traffic_light_y_distance < 200: + stopline = self.__stopline[0] + if self.traffic_light_y_distance < 220 and stopline > 10: return 10 - elif self.traffic_light_y_distance < 150: + elif self.traffic_light_y_distance < 170 and stopline > 7: return 0.0 else: - return self.__stopline[0] + return stopline else: return 0.0 From 1876ef2c7dac31accf6d2d6c1165af9ecde798b8 Mon Sep 17 00:00:00 2001 From: JuliusMiller Date: Tue, 19 Mar 2024 17:32:50 +0100 Subject: [PATCH 3/4] fix: adjust parameter --- code/planning/src/local_planner/motion_planning.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/code/planning/src/local_planner/motion_planning.py b/code/planning/src/local_planner/motion_planning.py index 82b777d0..77d1be11 100755 --- a/code/planning/src/local_planner/motion_planning.py +++ b/code/planning/src/local_planner/motion_planning.py @@ -33,7 +33,7 @@ class MotionPlanning(CompatibleNode): def __init__(self): super(MotionPlanning, self).__init__('MotionPlanning') self.role_name = self.get_param("role_name", "hero") - self.control_loop_rate = self.get_param("control_loop_rate", 0.1) + self.control_loop_rate = self.get_param("control_loop_rate", 0.05) self.target_speed = 0.0 self.__curr_behavior = None @@ -530,9 +530,9 @@ def __calc_virtual_change_point(self) -> float: def __calc_virtual_stopline(self) -> float: if self.__stopline[0] != np.inf and self.__stopline[1]: stopline = self.__stopline[0] - if self.traffic_light_y_distance < 220 and stopline > 10: + if self.traffic_light_y_distance < 250 and stopline > 10: return 10 - elif self.traffic_light_y_distance < 170 and stopline > 7: + elif self.traffic_light_y_distance < 180 and stopline > 7: return 0.0 else: return stopline From 17100dae7ed60de155d263846eff0d186edd2495 Mon Sep 17 00:00:00 2001 From: JuliusMiller Date: Fri, 22 Mar 2024 14:14:13 +0100 Subject: [PATCH 4/4] fix: deactivate dev-mode --- code/planning/launch/planning.launch | 4 ++-- code/planning/src/global_planner/global_planner.py | 2 +- code/planning/src/local_planner/motion_planning.py | 1 + 3 files changed, 4 insertions(+), 3 deletions(-) diff --git a/code/planning/launch/planning.launch b/code/planning/launch/planning.launch index 354b8d65..7437344e 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 361ab5b5..10cf33ed 100755 --- a/code/planning/src/global_planner/global_planner.py +++ b/code/planning/src/global_planner/global_planner.py @@ -78,7 +78,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/motion_planning.py b/code/planning/src/local_planner/motion_planning.py index deeb9b84..e54ba208 100755 --- a/code/planning/src/local_planner/motion_planning.py +++ b/code/planning/src/local_planner/motion_planning.py @@ -132,6 +132,7 @@ def __init__(self): Int16, f"/paf/{self.role_name}/Center/traffic_light_y_distance", self.__set_traffic_y_distance, + qos_profile=1) self.unstuck_distance_sub: Subscriber = self.new_subscription( Float32, f"/paf/{self.role_name}/unstuck_distance",