fix: ensure that dynamic line collision checking uses subsampling for…

… correct results (#43)

* fix: ensure that dynamic line collision checking uses subsampling for correct results

* chore: extend test suite

src/algorithms/replanning_rrt.py

@@ -86,8 +86,8 @@ def run(
         # traverse path and check if recomputation is required along each edge with respect to the dynamic obstacles
         if not quiet:
             print("Validating path...")
-        collision_free, save_idx, save_time = rrt.validate_path(
-            path=sol_path, start_time=query_time
+        collision_free, save_idx, save_idx_time, last_save, last_time = (
+            rrt.validate_path(path=sol_path, start_time=query_time, stepsize=stepsize)
         )
 
         if collision_free:
@@ -104,48 +104,20 @@ def run(
                 print(
                     "Path is not collision free, with first collision at edge with starting point: ",
                     rrt.tree[sol_path[save_idx]]["position"],
+                    "at time:",
+                    save_idx_time,
                 )
 
             # add all points up to the collision point to the final path (coordinates)
             new_path = prev_path + [
                 rrt.tree[sol_path[i]]["position"] for i in range(1, save_idx + 1)
             ]
 
-            # follow the next edge with fixed size steps and save the last collision-free point
-            save_node = new_path[-1]
-            next_node = rrt.tree[sol_path[save_idx + 1]]["position"]
-            delta_distance = next_node.distance(save_node)
-            num_steps = int(delta_distance / stepsize)
-            x_step = (next_node.x - save_node.x) / num_steps
-            y_step = (next_node.y - save_node.y) / num_steps
-
-            # track the position and time of the last node, which is not in collision
-            last_save = None
-            last_time = save_time
-
-            # iterate over the edge and check for the last save point on it
-            for i in range(1, num_steps):
-                sample = ShapelyPoint(
-                    save_node.x + i * x_step, save_node.y + i * y_step
-                )
-                sample_time = last_time + stepsize
-
-                # check if the sample is in collision
-                collision_free = self.env.static_collision_free(
-                    point=sample, query_time=sample_time
-                )
-
-                if collision_free:
-                    last_save = sample
-                    last_time = sample_time
-                else:
-                    break
-
             if last_save is None:
                 if (save_node.x, save_node.y) != start:
                     # if the last save node is not the start node, replan from the last save point
                     last_save = save_node
-                    last_time = save_time
+                    last_time = save_idx_time
 
                 elif in_recursion:
                     # to skip issues where RRT fails due an obstacle popping up around a point

src/algorithms/rrt.py

@@ -195,7 +195,9 @@ def rrt_find_path(self):
 
         return path[::-1]
 
-    def validate_path(self, path: List[int], start_time: float = 0.0):
+    def validate_path(
+        self, path: List[int], start_time: float = 0.0, stepsize: float = 1
+    ):
         """
         Validates the given path for collision with dynamics obstacles.
 
@@ -219,16 +221,16 @@ def validate_path(self, path: List[int], start_time: float = 0.0):
             edge = ShapelyLine([(parent.x, parent.y), (child.x, child.y)])
 
             # check for dynamic collisions
-            collision_free = self.env.dynamic_collision_free_ln(
-                edge, Interval(time, edge_end_time, closed="both")
+            collision_free, last_save, last_time = self.env.dynamic_collision_free_ln(
+                edge, Interval(time, edge_end_time, closed="both"), stepsize
             )
 
             if not collision_free:
-                return False, i, time
+                return False, i, time, last_save, last_time
 
             time = edge_end_time
 
-        return True, None, None
+        return True, None, None, None, None
 
     def __find_closest_neighbor(
         self, candidate: ShapelyPoint, rewiring: bool

src/envs/environment_instance.py

@@ -627,7 +627,7 @@ def collision_free_intervals_ln(
             return False, False, intervals
 
     def dynamic_collision_free_ln(
-        self, line: ShapelyLine, query_interval: Interval
+        self, line: ShapelyLine, query_interval: Interval, stepsize: float = 1
     ) -> bool:
         """
         Check if a given line is in collision with any dynamic obstacle in the environment.
@@ -649,17 +649,67 @@ def dynamic_collision_free_ln(
 
         # if no dynamic obstacles were found in the considered cells, return True
         if len(dynamic_ids) == 0:
-            return True
+            return True, None, None
 
         # if dynamic obstacles were found, check if the line is in collision with any of them
         for key in dynamic_ids:
             obstacle = self.dynamic_obstacles[key]
 
             if obstacle.is_active(query_interval=query_interval):
                 if obstacle.check_collision(shape=line):
-                    return False
+                    # The line overlaps in time and space with obstacle -> POSSIBLE COLLISION
+                    # --> verify collision with subsampling
+
+                    # keep track of last save subsample and time
+                    last_save = None
+                    last_save_time = query_interval.left
+
+                    # extract start and end points from line
+                    start = ShapelyPoint(line.coords[0][0], line.coords[0][1])
+                    end = ShapelyPoint(line.coords[1][0], line.coords[1][1])
+
+                    # apply subsampling to check for collision
+                    true_positive = False
+                    delta_distance = end.distance(start)
+                    num_steps = int(delta_distance / stepsize)
+                    x_step = (end.x - start.x) / num_steps
+                    y_step = (end.y - start.y) / num_steps
+
+                    for i in range(1, num_steps):
+                        sample = ShapelyPoint(
+                            start.x + i * x_step, start.y + i * y_step
+                        )
+                        sample_time = last_save_time + stepsize
 
-        return True
+                        # check if the sample is in collision
+                        collision_free = self.static_collision_free(
+                            point=sample, query_time=sample_time
+                        )
+
+                        if collision_free:
+                            last_save = sample
+                            last_save_time = sample_time
+                        else:
+                            true_positive = True
+                            break
+
+                    # edge is in collision at some intermediary position
+                    if true_positive:
+                        return False, last_save, last_save_time
+
+                    else:
+                        # check if the end of the edge is in collision
+                        collision_free = self.static_collision_free(
+                            point=end, query_time=query_interval.right
+                        )
+                        if not collision_free:
+                            return False, end, query_interval.right
+                        else:
+                            print(
+                                "Detected false positive in dynamic edge collision check with subsampling."
+                            )
+
+        return True, None, None
 
     def get_static_obs_free_volume(self):
         """

src/test/environment_instance_test.py

@@ -1796,3 +1796,76 @@ def test_free_space_computation(self):
         assert (
             abs(free_space - (10000 - math.pi - 20 - math.pi - 100 - 40 - math.pi)) < 1
         )
+
+    def test_dynamic_collision_free_line(self):
+        env_base = Environment()
+        obstacle = Polygon(
+            geometry=ShapelyPolygon([(10, 10), (20, 10), (20, 20), (10, 20)]),
+            radius=0.0,
+            time_interval=Interval(20, 30, closed="both"),
+        )
+        env_base.add_obstacles([obstacle])
+        env = EnvironmentInstance(
+            environment=env_base,
+            query_interval=Interval(0, 100, closed="both"),
+            scenario_range_x=(0, 100),
+            scenario_range_y=(0, 100),
+            resolution=10,
+        )
+
+        non_colliding_line = ShapelyLine([(0, 0), (5, 5)])
+        line = ShapelyLine([(5, 15), (25, 15)])
+        stepsize = 0.1
+
+        # Test 1: line is not in collision with obstacle at all
+        collision_free1, last_save, last_time = env.dynamic_collision_free_ln(
+            line=non_colliding_line,
+            query_interval=Interval(20, 30),
+            stepsize=stepsize,
+        )
+        assert collision_free1 == True
+        assert last_save is None
+        assert last_time is None
+
+        collision_free2, last_save, last_time = env.dynamic_collision_free_ln(
+            line=non_colliding_line,
+            query_interval=Interval(0, 20),
+            stepsize=stepsize,
+        )
+        assert collision_free2 == True
+        assert last_save is None
+        assert last_time is None
+
+        # Test 2: line with spatial intersection, but no overlap in temporal domain
+        collision_free3, last_save, last_time = env.dynamic_collision_free_ln(
+            line=line, query_interval=Interval(-10, 10), stepsize=stepsize
+        )
+        assert collision_free3 == True
+        assert last_save is None
+        assert last_time is None
+
+        # Test 3: line with spatial and temporal intersection, but only enters obstacle when inactive
+        collision_free4, last_save, last_time = env.dynamic_collision_free_ln(
+            line=line, query_interval=Interval(28, 48), stepsize=stepsize
+        )
+        assert collision_free4 == True
+        assert last_save is None
+        assert last_time is None
+
+        # Test 4: line with spatial and temporal intersection, and enters obstacle when active
+        collision_free5, last_save, last_time = env.dynamic_collision_free_ln(
+            line=line, query_interval=Interval(20, 40), stepsize=stepsize
+        )
+        assert collision_free5 == False
+        assert last_save is not None
+        assert last_save == ShapelyPoint(9.9, 15)
+        assert last_time is not None
+        assert (last_time - 24.9) < 1e-9
+
+        # Test 5: line with spatial and temporal intersection, and exits obstacle before active
+        collision_free6, last_save, last_time = env.dynamic_collision_free_ln(
+            line=line, query_interval=Interval(2, 22), stepsize=stepsize
+        )
+        assert collision_free6 == True
+        assert last_save is None
+        assert last_time is None