fix: intersection, lane_change, curve_detection

code/agent/config/rviz_config.rviz

@@ -65,7 +65,7 @@ Visualization Manager:
       Visibility:
         Grid: false
         Imu: false
-        Path: true
+        Path: false
         PointCloud2: false
         Value: true
       Zoom Factor: 1

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_no_light", convert_to_ms(50.0))
 
 # Exit
 
@@ -45,11 +41,11 @@ 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)
 
 # Wait
 

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

@@ -104,7 +104,6 @@ def update(self):
             "/paf/hero/Center/traffic_light_state")
         if light_status_msg is not None:
             self.traffic_light_status = get_color(light_status_msg.state)
-            rospy.logerr(f"Light Status approach: {self.traffic_light_status}")
             self.traffic_light_detected = True
 
         # Update stopline Info
@@ -138,7 +137,7 @@ def update(self):
                     not self.traffic_light_detected):
 
             rospy.loginfo("slowing down!")
-            self.curr_behavior_pub.publish(bs.int_app_no_sign.name)
+            self.curr_behavior_pub.publish(bs.int_app_to_stop.name)
 
         # approach slowly when traffic light is green as traffic lights are
         # higher priority than traffic signs this behavior is desired
@@ -230,6 +229,7 @@ def setup(self, timeout):
                                                  "curr_behavior", String,
                                                  queue_size=1)
         self.blackboard = py_trees.blackboard.Blackboard()
+        self.red_light_flag = False
         return True
 
     def initialise(self):
@@ -243,6 +243,7 @@ def initialise(self):
         :return: True
         """
         rospy.loginfo("Wait Intersection")
+        self.red_light_flag = False
         return True
 
     def update(self):
@@ -279,9 +280,13 @@ def update(self):
 
         if light_status_msg is not None:
             traffic_light_status = get_color(light_status_msg.state)
-            rospy.logerr(f"Light Status wait: {traffic_light_status}")
             if traffic_light_status == "red" or \
                     traffic_light_status == "yellow":
+                self.red_light_flag = True
+                rospy.loginfo(f"Light Status: {traffic_light_status}")
+                self.curr_behavior_pub.publish(bs.int_wait.name)
+                return py_trees.common.Status.RUNNING
+            elif self.red_light_flag and traffic_light_status != "green":
                 rospy.loginfo(f"Light Status: {traffic_light_status}")
                 self.curr_behavior_pub.publish(bs.int_wait.name)
                 return py_trees.common.Status.RUNNING
@@ -358,14 +363,13 @@ def initialise(self):
         light_status_msg = self.blackboard.get(
             "/paf/hero/Center/traffic_light_state")
         if light_status_msg is None:
-            self.curr_behavior_pub.publish(bs.int_enter_no_light.name)
+            self.curr_behavior_pub.publish(bs.int_enter.name)
             return True
 
         traffic_light_status = get_color(light_status_msg.state)
-        rospy.logerr(f"Light Status ENTER: {traffic_light_status}")
 
         rospy.loginfo(f"Light Status: {traffic_light_status}")
-        self.curr_behavior_pub.publish(bs.int_enter_light.name)
+        self.curr_behavior_pub.publish(bs.int_enter.name)
 
     def update(self):
         """
@@ -391,7 +395,7 @@ def update(self):
             # not next_waypoint_msg.isStopLine:
         if next_waypoint_msg.distance < 5:
             rospy.loginfo("Drive through intersection!")
-            # self.update_local_path(leave_intersection=True)
+            self.curr_behavior_pub.publish(bs.int_enter.name)
             return py_trees.common.Status.RUNNING
         else:
             return py_trees.common.Status.SUCCESS

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

@@ -70,7 +70,7 @@ def initialise(self):
         self.change_detected = False
         self.change_distance = np.inf
         self.virtual_change_distance = np.inf
-        self.curr_behavior_pub.publish(bs.lc_init.name)
+        self.curr_behavior_pub.publish(bs.lc_app_init.name)
 
     def update(self):
         """
@@ -111,13 +111,13 @@ def update(self):
             else:
                 distance_lidar = None
 
-            if distance_lidar is not None and distance_lidar.min_range > 15.0:
+            distance_lidar = 20  # Remove and adjust to check for cars behind
+
+            if distance_lidar is not None and distance_lidar > 15.0:
                 rospy.loginfo("Change is free not slowing down!")
-                # self.update_local_path(leave_intersection=True)
                 return py_trees.common.Status.SUCCESS
             else:
                 rospy.loginfo("Change blocked slowing down")
-                self.curr_behavior_pub.publish(bs.lc_app_blocked.name)
 
         # get speed
         speedometer = self.blackboard.get("/carla/hero/Speed")
@@ -242,10 +242,12 @@ def update(self):
         else:
             distance_lidar = None
 
+        distance_lidar = 20  # Remove to wait
+
         change_clear = False
         if distance_lidar is not None:
             # if distance smaller than 15m, change is blocked
-            if distance_lidar.min_range < 15.0:
+            if distance_lidar < 15.0:
                 change_clear = False
             else:
                 change_clear = True