192 feature finish overtake behavior

.flake8

@@ -2,4 +2,6 @@
 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/__init__.py
+         code/planning/__init__.py,
+         doc/02_development/templates/template_class_no_comments.py,
+         doc/02_development/templates/template_class.py

build/docker-compose.yml

@@ -62,7 +62,7 @@ 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 && 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:

code/agent/launch/dev.launch

@@ -32,7 +32,7 @@
     <arg name="objects_definition_file" value='$(find agent)/config/dev_objects.json'/>
   </include>
 
-  <include file="$(find agent)/launch/agent_manual.launch">
+  <include file="$(find agent)/launch/agent.launch">
     <arg name='role_name' value='$(arg role_name)'/>
   </include>
 

code/perception/src/vision_node.py

@@ -191,7 +191,7 @@ def setup_camera_publishers(self):
 
     def setup_object_distance_publishers(self):
         self.distance_publisher = self.new_publisher(
-            msg_type=numpy_msg(Float32MultiArray),
+            msg_type=Float32MultiArray,
             topic=f"/paf/{self.role_name}/{self.side}/object_distance",
             qos_profile=1)
 
@@ -227,6 +227,7 @@ def handle_camera_image(self, image):
         if side == "Right":
             self.publisher_right.publish(img_msg)
 
+        # locals().clear()
         # print(f"Published Image on Side: {side}")
         pass
 
@@ -254,6 +255,7 @@ def handle_dist_array(self, dist_array):
                                       desired_encoding='passthrough')
         # print("RECEIVED DIST")
         self.dist_arrays = dist_array
+        # locals().clear()
 
     def predict_torch(self, image):
         self.model.eval()
@@ -285,40 +287,6 @@ def predict_ultralytics(self, image):
 
         output = self.model(cv_image, half=True, verbose=False)
 
-        """distance_output = []
-        c_boxes = []
-        c_labels = []
-        for r in output:
-            boxes = r.boxes
-            for box in boxes:
-                cls = box.cls.item()
-                pixels = box.xyxy[0]
-                print(pixels)
-                if len(self.depth_images) > 0:
-                    distances = np.asarray(
-                        [self.depth_images[i][int(pixels[1]):int(pixels[3]):1,
-                                              int(pixels[0]):int(pixels[2]):1]
-                            for i in range(len(self.depth_images))])
-                    non_zero_filter = distances[distances != 0]
-
-                    if len(non_zero_filter) > 0:
-                        obj_dist = np.min(non_zero_filter)
-                    else:
-                        obj_dist = np.inf
-
-                    c_boxes.append(torch.tensor(pixels))
-                    c_labels.append(f"Class: {cls}, Meters: {obj_dist}")
-                    distance_output.append([cls, obj_dist])
-
-        # print(distance_output)
-        # self.logerr(distance_output)
-        self.distance_publisher.publish(
-            Float32MultiArray(data=distance_output))
-
-        transposed_image = np.transpose(cv_image, (2, 0, 1))
-        image_np_with_detections = torch.tensor(transposed_image,
-                                                dtype=torch.uint8)"""
-
         # handle distance of objects
         distance_output = []
         c_boxes = []
@@ -339,14 +307,30 @@ def predict_ultralytics(self, image):
                     distances_copy[distances_copy == 0] = np.inf
 
                     if len(non_zero_filter) > 0:
-                        sorted_indices = np.argsort(distances_copy[:, :, 0],
-                                                    axis=None)
-                        x1, y1 = np.unravel_index(sorted_indices[0],
-                                                  distances_copy.shape[:2])
-                        x2, y2 = np.unravel_index(sorted_indices[1],
+                        min_x_sorted_indices = np.argsort(
+                            distances_copy[:, :, 0],
+                            axis=None)
+                        x1, y1 = np.unravel_index(min_x_sorted_indices[0],
                                                   distances_copy.shape[:2])
+                        """x2, y2 = np.unravel_index(min_x_sorted_indices[1],
+                                                  distances_copy.shape[:2])"""
+
+                        abs_distance_copy = np.abs(distances_copy.copy())
+                        min_y_sorted_indices = np.argsort(
+                            abs_distance_copy[:, :, 1],
+                            axis=None)
+                        x3, y3 = np.unravel_index(min_y_sorted_indices[0],
+                                                  abs_distance_copy.shape[:2])
+                        """
+                        x4, y4 = np.unravel_index(min_y_sorted_indices[1],
+                                                  abs_distance_copy.shape[:2])
+                        """
+
                         obj_dist1 = distances_copy[x1][y1].copy()
-                        obj_dist2 = distances_copy[x2][y2].copy()
+                        # obj_dist2 = distances_copy[x2][y2].copy()
+                        obj_dist3 = distances_copy[x3][y3].copy()
+                        # obj_dist4 = distances_copy[x4][y4].copy()
+                        # print(obj_dist1, obj_dist3)
 
                         abs_distance = np.sqrt(
                             obj_dist1[0]**2 +
@@ -355,38 +339,53 @@ def predict_ultralytics(self, image):
 
                         # create 2d glass plane at object
                         # with box dimension
-                        scale_width = abs(obj_dist1[1] - obj_dist2[1])\
-                            / abs(y1-y2)
-                        scale_height = abs(obj_dist1[2] - obj_dist2[2])\
-                            / abs(x1-x2)
-                        width = distances_copy.shape[1] * scale_width
-                        height = distances_copy.shape[0] * scale_height
-
-                        # upper left
-                        ul_x = obj_dist1[0]
-                        ul_y = obj_dist1[1] - (-y1 + scale_width)
-                        ul_z = obj_dist1[2] - (-x1 + scale_height)
-
-                        # lower right
-                        lr_x = obj_dist1[0]
-                        lr_y = ul_y + width
-                        lr_z = ul_z + height
+                        # width_diff = abs(y1-y2)
+                        # height_diff = abs(x1-x2)
+
+                        """if width_diff > 0 and height_diff > 0:
+                            scale_width = abs(obj_dist1[1] - obj_dist2[1])\
+                                / width_diff
+                            scale_height = abs(obj_dist1[2] - obj_dist2[2])\
+                                / height_diff
+                            width = distances_copy.shape[1] * scale_width
+                            height = distances_copy.shape[0] * scale_height
+
+                            # upper left
+                            ul_x = obj_dist1[0]
+                            ul_y = obj_dist1[1] - (-y1 + scale_width)
+                            ul_z = obj_dist1[2] - (-x1 + scale_height)
+
+                            # lower right
+                            lr_x = obj_dist1[0]
+                            lr_y = ul_y + width
+                            lr_z = ul_z + height
+
+                            # -5 < y < 8"""
+
+                        distance_output.append(float(cls))
+                        distance_output.append(float(obj_dist1[0]))
+                        distance_output.append(float(obj_dist3[1]))
 
                     else:
                         obj_dist1 = (np.inf, np.inf, np.inf)
+                        obj_dist3 = (np.inf, np.inf, np.inf)
                         abs_distance = np.inf
+                        """distance_output.append(float(cls))
+                        distance_output.append(float(abs_distance))
+                        distance_output.append(float(np.inf))
+                        distance_output.append(float(np.inf))
+                        distance_output.append(float(np.inf))
+                        distance_output.append(float(np.inf))
+                        distance_output.append(float(np.inf))
+                        distance_output.append(float(np.inf))"""
 
                     c_boxes.append(torch.tensor(pixels))
                     c_labels.append(f"Class: {cls},"
                                     f"Meters: {round(abs_distance, 2)},"
                                     f"({round(float(obj_dist1[0]), 2)},"
-                                    f"{round(float(obj_dist1[1]), 2)},"
-                                    f"{round(float(obj_dist1[2]), 2)})")
-                    distance_output.append([cls,
-                                            abs_distance,
-                                            ul_x, ul_y, ul_z,
-                                            lr_x, lr_y, lr_z])
+                                    f"{round(float(obj_dist3[1]), 2)})")
 
+        # print("DISTANCE_ARRAY: ", distance_output)
         self.distance_publisher.publish(
            Float32MultiArray(data=distance_output))
 
@@ -398,7 +397,6 @@ def predict_ultralytics(self, image):
             self.process_traffic_lights(output[0], cv_image, image.header)
 
         c_boxes = torch.stack(c_boxes)
-        # print(image_np_with_detections.shape, c_boxes.shape, c_labels)
         box = draw_bounding_boxes(image_np_with_detections,
                                   c_boxes,
                                   c_labels,
@@ -408,7 +406,9 @@ def predict_ultralytics(self, image):
         np_box_img = np.transpose(box.detach().numpy(),
                                   (1, 2, 0))
         box_img = cv2.cvtColor(np_box_img, cv2.COLOR_BGR2RGB)
+        # locals().clear()
         return box_img
+
         # return output[0].plot()
 
     def process_traffic_lights(self, prediction, cv_image, image_header):
@@ -438,6 +438,8 @@ def process_traffic_lights(self, prediction, cv_image, image_header):
             traffic_light_image.header = image_header
             self.traffic_light_publisher.publish(traffic_light_image)
 
+        # locals().clear()
+
     def create_mask(self, input_image, model_output):
         output_predictions = torch.argmax(model_output, dim=0)
         for i in range(21):

code/planning/launch/planning.launch

@@ -1,19 +1,19 @@
 <launch>
     <node pkg="planning" type="collision_check.py" name="CollisionCheck" output="screen">
            <param name="role_name" value="hero" />
-           <param name="control_loop_rate" value="0.3" />
+           <param name="control_loop_rate" value="0.05" />
     </node>
     <node pkg="planning" type="ACC.py" name="ACC" output="screen">
         <param name="role_name" value="hero" />
-        <param name="control_loop_rate" value="0.5" />
+        <param name="control_loop_rate" value="0.3" />
     </node>
     <!-- <node pkg="local_planner" type="local_planner/dev_collision_publisher.py" name="DevCollisionCheck" output="screen">
         <param name="role_name" value="hero" />
         <param name="control_loop_rate" value="0.3" />
     </node> -->
     <node pkg="planning" type="motion_planning.py" name="MotionPlanning" output="screen">
            <param name="role_name" value="hero" />
-           <param name="control_loop_rate" value="0.3" />
+           <param name="control_loop_rate" value="0.1" />
     </node>
 
     <!-- <node pkg="planning" type="dev_global_route.py" name="DevGlobalRoute" output="screen">

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

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

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

@@ -47,7 +47,7 @@ def convert_to_ms(speed):
 # TODO: Find out purpose of v_stop in lane_change (lines: 105 - 128)
 lc_app_blocked = Behavior("lc_app_blocked", -2)
 
-lc_app_free = Behavior("lc_app_free", convert_to_ms(20))
+lc_app_free = Behavior("lc_app_free", convert_to_ms(30.0))
 
 # Wait
 lc_wait = Behavior("lc_wait", 0)
@@ -63,6 +63,19 @@ def convert_to_ms(speed):
 lc_exit = Behavior("lc_exit", -1)  # Get SpeedLimit dynamically
 
 
+# Overtake
+
+# Approach
+ot_app_blocked = Behavior("ot_app_blocked", -2)
+ot_app_free = Behavior("ot_app_free", -1)
+# Wait
+ot_wait_stopped = Behavior("ot_wait_stopped", convert_to_ms(0.0))
+ot_wait_free = Behavior("ot_wait_free", -1)
+# Enter
+ot_enter_init = Behavior("ot_enter_init", -1)
+ot_enter_slow = Behavior("ot_enter_slow", -2)
+# Exit
+ot_leave = Behavior("ot_leave", -1)
 # Cruise
 
 cruise = Behavior("Cruise", -1)