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/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 3c60ecdd..46616694 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..10cf33ed 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, diff --git a/code/planning/src/local_planner/motion_planning.py b/code/planning/src/local_planner/motion_planning.py index b54468b1..e54ba208 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 @@ -36,7 +36,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 @@ -57,12 +57,11 @@ def __init__(self): self.__corners = None self.__in_corner = False self.calculated = False - + self.traffic_light_y_distance = np.inf # unstuck routine variables self.unstuck_distance = None self.unstuck_overtake_flag = False self.init_overtake_pos = None - # Subscriber self.test_sub = self.new_subscription( Float32, @@ -123,12 +122,17 @@ 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) self.unstuck_distance_sub: Subscriber = self.new_subscription( Float32, f"/paf/{self.role_name}/unstuck_distance", @@ -208,6 +212,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 @@ -549,14 +557,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 @@ -565,8 +573,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: @@ -575,10 +582,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 @@ -593,7 +598,13 @@ 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] + stopline = self.__stopline[0] + if self.traffic_light_y_distance < 250 and stopline > 10: + return 10 + elif self.traffic_light_y_distance < 180 and stopline > 7: + return 0.0 + else: + return stopline else: return 0.0