159 feature intersection behaviour testing (#173)

* feat: Implement TrafficLightNode

* fix: smoother approach

* fix: intersection, lane_change, curve_detection

* fix: add green_light_counter

---------

Co-authored-by: MaxJa4 <[email protected]>
Co-authored-by: MaxJa4 <[email protected]>
Co-authored-by: Julius Miller <[email protected]>
Co-authored-by: samuelkuehnel <[email protected]>

.flake8

@@ -1,4 +1,5 @@
 [flake8]
 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/src/behavior_agent/behaviours,
+         code/planning/__init__.py

build/docker-compose.yml

@@ -62,8 +62,9 @@ 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=carla-simulator --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"
     environment:

build/docker/agent/Dockerfile

@@ -162,6 +162,21 @@ RUN source ~/.bashrc && pip install -r /workspace/requirements.txt
 # Add agent code
 COPY --chown=$USERNAME:$USERNAME ./code /workspace/code/
 
+# Install Frenet Optimal Trajectory Planner 
+RUN sudo mkdir /erdos
+RUN sudo mkdir /erdos/dependencies
+RUN sudo mkdir /erdos/dependencies/frenet_optimal_trajectory_planner
+# Needed to resolve dependencies correctly inside freent_optimal_trajectory_planner 
+ENV PYLOT_HOME=/erdos
+
+ENV FREENET_HOME=/erdos/dependencies/frenet_optimal_trajectory_planner
+RUN sudo chown $USERNAME:$USERNAME $PYLOT_HOME
+RUN sudo chown $USERNAME:$USERNAME $FREENET_HOME
+
+RUN git clone https://github.com/erdos-project/frenet_optimal_trajectory_planner.git $FREENET_HOME
+RUN cd $FREENET_HOME && source ./build.sh
+
+ENV PYTHONPATH=$PYTHONPATH:/erdos/dependencies
 # Link code into catkin workspace
 RUN ln -s /workspace/code /catkin_ws/src
 
@@ -170,6 +185,8 @@ RUN source /opt/ros/noetic/setup.bash && catkin_make
 
 ADD ./build/docker/agent/entrypoint.sh /entrypoint.sh
 
+
+
 # set the default working directory to the code
 WORKDIR /workspace/code
 

code/agent/config/rviz_config.rviz

@@ -67,7 +67,7 @@ Visualization Manager:
         Imu: false
         Path: false
         PointCloud2: false
-        Value: false
+        Value: true
       Zoom Factor: 1
     - Class: rviz/Image
       Enabled: true
@@ -166,7 +166,7 @@ Visualization Manager:
       Offset:
         X: 0
         Y: 0
-        Z: 39
+        Z: 703
       Pose Color: 255; 85; 255
       Pose Style: None
       Queue Size: 10

code/perception/src/traffic_light_detection/src/traffic_light_detection/traffic_light_inference.py

@@ -64,7 +64,7 @@ def __call__(self, img):
         else:
             out = self.model(img)
         _, prediction = torch.max(out.data, 1)
-        return prediction.item()
+        return (prediction.item(), out.data.cpu().numpy())
 
 
 # main function for testing purposes

code/perception/src/traffic_light_node.py

@@ -1,5 +1,8 @@
 #!/usr/bin/env python3
 
+from datetime import datetime
+import threading
+from time import sleep
 from ros_compatibility.node import CompatibleNode
 import ros_compatibility as roscomp
 from rospy.numpy_msg import numpy_msg
@@ -18,6 +21,9 @@ def __init__(self, name, **kwargs):
         self.role_name = self.get_param("role_name", "hero")
         self.side = self.get_param("side", "Center")
         self.classifier = TrafficLightInference(self.get_param("model", ""))
+        self.last_info_time: datetime = None
+        self.last_state = None
+        threading.Thread(target=self.auto_invalidate_state).start()
 
         # publish / subscribe setup
         self.setup_camera_subscriptions()
@@ -38,14 +44,38 @@ def setup_traffic_light_publishers(self):
             qos_profile=1
         )
 
+    def auto_invalidate_state(self):
+        while True:
+            sleep(1)
+
+            if self.last_info_time is None:
+                continue
+
+            if (datetime.now() - self.last_info_time).total_seconds() >= 2:
+                msg = TrafficLightState()
+                msg.state = 0
+                self.traffic_light_publisher.publish(msg)
+                self.last_info_time = None
+
     def handle_camera_image(self, image):
-        result = self.classifier(self.bridge.imgmsg_to_cv2(image))
+        result, data = self.classifier(self.bridge.imgmsg_to_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
 
-        # 1: Green, 2: Red, 4: Yellow, 0: Unknown
-        msg = TrafficLightState()
-        msg.state = result if result in [1, 2, 4] else 0
+        state = result if result in [1, 2, 4] else 0
+        if self.last_state == state:
+            # 1: Green, 2: Red, 4: Yellow, 0: Unknown
+            msg = TrafficLightState()
+            msg.state = state
+            self.traffic_light_publisher.publish(msg)
+        else:
+            self.last_state = state
 
-        self.traffic_light_publisher.publish(msg)
+        # Automatically invalidates state (state=0) in auto_invalidate_state()
+        if state != 0:
+            self.last_info_time = datetime.now()
 
     def run(self):
         self.spin()

code/perception/src/vision_node.py

@@ -191,14 +191,19 @@ 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
 
-        min_x = 550
-        max_x = 700
-        min_prob = 0.35
+        max_y = 360  # middle of image
+        min_prob = 0.30
 
         for index in indices:
             box = prediction.boxes.cpu().data.numpy()[index]
 
-            if box[0] < min_x or box[2] > max_x or box[4] < min_prob:
+            if box[4] < min_prob:
+                continue
+
+            if (box[2] - box[0]) * 1.5 > box[3] - box[1]:
+                continue  # ignore horizontal boxes
+
+            if box[1] > max_y:
                 continue
 
             box = box[0:4].astype(int)

code/planning/__init__.py

@@ -0,0 +1 @@
+import behavior_agent

code/planning/src/behavior_agent/behaviours/__init__.py

@@ -0,0 +1,3 @@
+from . import intersection, lane_change, maneuvers, meta, road_features
+from . import topics2blackboard, traffic_objects
+from . import behavior_speed

code/planning/src/behavior_agent/behaviours/behavior_speed.py

@@ -20,7 +20,7 @@ def convert_to_ms(speed):
 int_app_init = Behavior("int_app_init", convert_to_ms(30.0))
 
 # No Traffic Light or Sign -> stop dynamically at Stopline
-int_app_no_sign = Behavior("int_app_no_sign", -2)
+int_app_to_stop = Behavior("int_app_to_stop", -2)
 
 int_app_green = Behavior("int_app_green", convert_to_ms(30.0))
 
@@ -30,11 +30,7 @@ def convert_to_ms(speed):
 
 # Enter
 
-int_enter_no_light = Behavior("int_enter_no_light", convert_to_ms(50.0))
-
-int_enter_empty_str = Behavior("int_enter_empty_string", convert_to_ms(18.0))
-
-int_enter_light = Behavior("int_enter_light", convert_to_ms(50.0))
+int_enter = Behavior("int_enter", convert_to_ms(50.0))
 
 # Exit
 
@@ -45,13 +41,16 @@ def convert_to_ms(speed):
 
 # Approach
 
-lc_app_init = Behavior("lc_app_blocked", convert_to_ms(30.0))
+lc_app_init = Behavior("lc_app_init", convert_to_ms(30.0))
 
 
 # TODO: Find out purpose of v_stop in lane_change (lines: 105 - 128)
-lc_app_blocked = Behavior("lc_app_blocked", 0.5)
+lc_app_blocked = Behavior("lc_app_blocked", -2)
+
+lc_app_free = Behavior("lc_app_free", convert_to_ms(20))
 
 # Wait
+lc_wait = Behavior("lc_wait", 0)
 
 # Has a publisher but doesnt publish anything ??