210 feature integrate new traffic light features

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,

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
 

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

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,

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
 """
 
 

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,

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