726 overtake lane free motion aware

code/mapping/config/mapping.cfg

@@ -25,7 +25,7 @@ tab_filters.add("enable_merge_filter", bool_t, 0, "Enable or disable the merging
 tab_filters.add("merge_growth_distance", double_t, 0, "Amount shapes grow before merging in meters.", 0.3, 0.0, 5.0)
 tab_filters.add("min_merging_overlap_percent", double_t, 0, "Min overlap of the grown shapes in percent.", 0.5, 0.0, 1.0)
 tab_filters.add("min_merging_overlap_area", double_t, 0, "Min overlap of the grown shapes in m2.", 0.5, 0.0, 5.0)
-tab_filters.add("polygon_simplify_tolerance", double_t, 0, "The polygon simplify tolerance.", 0.01, 0.1, 1.0)
+tab_filters.add("polygon_simplify_tolerance", double_t, 0, "The polygon simplify tolerance.", 0.1, 0.01, 1.0)
 
 tab_lidar = gen.add_group("Lidar", type="tab")
 tab_lidar.add("lidar_z_min", double_t, 0, "Excludes lidar points below this height.", -1.5, -10, 2.0)

code/mapping/ext_modules/mapping_common/entity.py

@@ -480,10 +480,13 @@ def get_global_x_velocity(self) -> Optional[float]:
         return velocity
 
     def get_delta_forward_velocity_of(self, other: "Entity") -> Optional[float]:
-        """Calculates the delta velocity compared to other in the heading of self
+        """Calculates the delta velocity compared to other in the heading of self.
+            This function is only for objects in front. If the entity is behind this
+            value has to be inverted. Use the "get_delta_velocity_of" function for this
+            case.
 
-        - result > 0: other moves away from self
-        - result < 0: other moves nearer to self
+        - result > 0: other moves in the forward direction of self
+        - result < 0: other moves in the backward direction of self
 
         Args:
             other (Entity)
@@ -501,6 +504,29 @@ def get_delta_forward_velocity_of(self, other: "Entity") -> Optional[float]:
         relative_motion = other_motion_in_self_coords - self.motion.linear_motion
         return relative_motion.x()
 
+    def get_delta_velocity_of(self, other: "Entity") -> Optional[float]:
+        """Calculates the delta velocity compared to other in the heading of self
+
+        - result > 0: other moves away from self
+        - result < 0: other moves nearer to self
+
+        Args:
+            other (Entity)
+
+        Returns:
+            Optional[float]: Delta velocity if both entities have one.
+        """
+        forward_velocity = self.get_delta_forward_velocity_of(other)
+        if forward_velocity is None:
+            return None
+
+        into_self_local: Transform2D = self.transform.inverse() * other.transform
+        other_position_in_self_coords: Vector2 = into_self_local.translation()
+
+        if other_position_in_self_coords.x() < 0.0:
+            return -forward_velocity
+        return forward_velocity
+
     def get_width(self) -> float:
         """Returns the local width (y-bounds) of the entity
 

code/mapping/ext_modules/mapping_common/map.py

@@ -22,6 +22,7 @@
 
 
 class LaneFreeState(Enum):
+    TO_BE_CHECKED = 2
     FREE = 1
     BLOCKED = 0
     MISSING_LANEMARK_ERR = -1
@@ -483,6 +484,7 @@ def is_lane_free(
         min_coverage_percent: float = 0.0,
         min_coverage_area: float = 0.0,
         lane_angle: float = 5.0,
+        motion_aware: bool = True,
     ) -> Tuple[LaneFreeState, Optional[shapely.Geometry]]:
         """Returns if a lane left or right of our car is free.
         There are three check methods available:
@@ -510,23 +512,18 @@ def is_lane_free(
             with the checkbox in percent. Defaults to 0.0.
         - min_coverage_area (float, optional): how much an entity must collide
             with the checkbox in m2. Defaults to 0.0.
+        - motion_aware (bool, optional): if true, the lane check will be aware of
+            the motion of the entities. Defaults to True.
 
         Default is "rectangle".
         Returns:
             Tuple[LaneFreeState, Optional[shapely.Geometry]]:
             return LaneFreeState and if available the checkbox shape
         """
-        if check_method == "rectangle":
-            return self.is_lane_free_rectangle(
-                right_lane=right_lane,
-                lane_length=lane_length,
-                lane_transform=lane_transform,
-                reduce_lane=reduce_lane,
-                min_coverage_percent=min_coverage_percent,
-                min_coverage_area=min_coverage_area,
-            )
-        elif check_method == "lanemarking":
-            return self.is_lane_free_lanemarking(
+        lane_state = LaneFreeState.TO_BE_CHECKED
+        lane_box = None
+        if check_method in ["lanemarking", "fallback"]:
+            lane_state, lane_box = self.is_lane_free_lanemarking(
                 right_lane=right_lane,
                 lane_length=lane_length,
                 lane_transform=lane_transform,
@@ -535,32 +532,63 @@ def is_lane_free(
                 min_coverage_area=min_coverage_area,
                 lane_angle=lane_angle,
             )
-        elif check_method == "fallback":
-            lane_free_state, lane_check_shape = self.is_lane_free_lanemarking(
+
+        if check_method == "rectangle" or (
+            check_method == "fallback" and lane_state.is_error()
+        ):
+            lane_state, lane_box = self.is_lane_free_rectangle(
                 right_lane=right_lane,
                 lane_length=lane_length,
                 lane_transform=lane_transform,
                 reduce_lane=reduce_lane,
                 min_coverage_percent=min_coverage_percent,
                 min_coverage_area=min_coverage_area,
-                lane_angle=lane_angle,
             )
 
-            # return value of is_lane_free_lanemarking function of value is plausible
-            if not lane_free_state.is_error():
-                return lane_free_state, lane_check_shape
+        if lane_state.is_error():
+            return lane_state, None
 
-            # use is_lane_free_rectangle function as fallback
-            return self.is_lane_free_rectangle(
-                right_lane=right_lane,
-                lane_length=lane_length,
-                lane_transform=lane_transform,
-                reduce_lane=reduce_lane,
-                min_coverage_percent=min_coverage_percent,
-                min_coverage_area=min_coverage_area,
-            )
+        if lane_box is None or lane_state != LaneFreeState.TO_BE_CHECKED:
+            return LaneFreeState.SHAPE_ERR, None
+
+        colliding_entities = self.get_overlapping_entities(
+            lane_box,
+            min_coverage_percent=min_coverage_percent,
+            min_coverage_area=min_coverage_area,
+        )
+
+        hero = self.map.hero()
+        if motion_aware and hero is not None:
+            for i in range(len(colliding_entities) - 1, -1, -1):
+                entity = colliding_entities[i]
 
-        # check_method == "trajectory": not implemented yet
+                forward_velocity = hero.get_delta_forward_velocity_of(entity.entity)
+                if forward_velocity is None:
+                    continue
+
+                into_self_local: Transform2D = (
+                    hero.transform.inverse() * entity.entity.transform
+                )
+                other_position_in_self_coords: Vector2 = into_self_local.translation()
+
+                # if the entity is behind the car and moving away from the car
+                # it is not considered as a colliding entity
+                # To not filter any parking cars, the threshold is set to abs(0.5).
+                if other_position_in_self_coords.x() < 0.0 and forward_velocity < -0.5:
+                    del colliding_entities[i]
+                    continue
+
+                # car is moving in our direction and is in front of us
+                # car needs to be at least 1.5 m away from us
+                if other_position_in_self_coords.x() > 1.5 and forward_velocity > 0.5:
+                    del colliding_entities[i]
+                    continue
+
+        # if there are colliding entities, the lane is not free
+        if not colliding_entities:
+            return LaneFreeState.FREE, lane_box
+
+        return LaneFreeState.BLOCKED, lane_box
 
     def is_lane_free_rectangle(
         self,
@@ -611,19 +639,9 @@ def is_lane_free_rectangle(
         )
 
         # converts lane box Rectangle to a shapely Polygon
-        lane_box_shapely = lane_box_shape.to_shapely(Transform2D.identity())
-
-        # get entities that are colliding with the checkbox entity
-        colliding_entities = self.get_overlapping_entities(
-            lane_box_shapely,
-            min_coverage_percent=min_coverage_percent,
-            min_coverage_area=min_coverage_area,
-        )
+        lane_box = lane_box_shape.to_shapely(Transform2D.identity())
 
-        # if list with lane box intersection is empty --> lane is free
-        if not colliding_entities:
-            return LaneFreeState.FREE, lane_box_shapely
-        return LaneFreeState.BLOCKED, lane_box_shapely
+        return LaneFreeState.TO_BE_CHECKED, lane_box
 
     def is_lane_free_lanemarking(
         self,
@@ -713,21 +731,7 @@ def is_lane_free_lanemarking(
         if not shapely.is_valid(lane_box):
             return LaneFreeState.SHAPE_ERR, None
 
-        # get entities that are colliding with the checkbox entity
-        # TODO: motion detection check could be done here also,
-        # deprecated function could be found in commit
-        # d6d246fe181d6c60a1de33e578f2d4a47cb05ed4
-        colliding_entities = self.get_overlapping_entities(
-            lane_box,
-            min_coverage_percent=min_coverage_percent,
-            min_coverage_area=min_coverage_area,
-        )
-
-        # if there are colliding entities, the lane is not free
-        if not colliding_entities:
-            return LaneFreeState.FREE, lane_box
-        else:
-            return LaneFreeState.BLOCKED, lane_box
+        return LaneFreeState.TO_BE_CHECKED, lane_box
 
     def get_nearest_entity(
         self,

code/planning/src/behavior_agent/behaviors/overtake.py

@@ -280,7 +280,7 @@ def initialise(self):
         global OVERTAKE_FREE
         self.ot_distance = 30
         self.ot_counter = 0
-        self.clear_distance = 45
+        self.clear_distance = 50
         OVERTAKE_FREE = False
 
     def update(self):
@@ -340,7 +340,7 @@ def update(self):
             ot_free, ot_mask = tree.is_lane_free(
                 right_lane=False,
                 lane_length=self.clear_distance,
-                lane_transform=15.0,
+                lane_transform=10.0,
                 check_method="fallback",
             )
             if isinstance(ot_mask, shapely.Polygon):
@@ -415,7 +415,7 @@ def setup(self, timeout):
     def initialise(self):
         rospy.loginfo("Waiting for Overtake")
         # slightly less distance since we have already stopped
-        self.clear_distance = 45
+        self.clear_distance = 50
         self.ot_counter = 0
         self.ot_gone = 0
         return True
@@ -484,7 +484,7 @@ def update(self):
         ot_free, ot_mask = tree.is_lane_free(
             right_lane=False,
             lane_length=self.clear_distance,
-            lane_transform=15.0,
+            lane_transform=10.0,
             check_method="fallback",
         )