feat: Collision detection pedestrians, bikes and more (#225)

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

@@ -129,9 +129,6 @@ def update(self):
             self.virtual_stopline_distance = self.stop_distance
         else:
             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

code/planning/src/local_planner/motion_planning.py

@@ -559,7 +559,6 @@ def __calc_speed_to_stop_intersection(self) -> float:
         target_distance = 5.0
         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(stopline / 0.8))
         if v_stop > bs.int_app_init.speed:
@@ -587,7 +586,7 @@ def __calc_speed_to_stop_overtake(self) -> float:
         if stopline < 6.0:
             v_stop = 0.0
 
-        self.logerr(f"Speed ovt: {v_stop}")
+        # self.logerr(f"Speed ovt: {v_stop}")
         return v_stop
 
     def __calc_virtual_change_point(self) -> float:

code/planning/src/local_planner/utils.py

@@ -215,8 +215,11 @@ def filter_vision_objects(float_array, oncoming):
     float_array = float_array[~np.any(np.isinf(float_array), axis=1), :]
     if float_array.size == 0:
         return None
-    # Filter out all objects that are not cars
-    all_cars = float_array[np.where(float_array[:, 0] == 2)]
+    # Filter out all objects that are not cars, Persons, Bycicles,
+    # Motorbikes, Busses or Trucks
+    all_cars = float_array[np.where(float_array[:, 0] <= 7)]
+    all_cars = all_cars[np.where(all_cars[:, 0] != 6)]
+    all_cars = all_cars[np.where(all_cars[:, 0] != 4)]
 
     # Get cars that are on our lane
     if oncoming: