in_collision fix

Author: omigeft

src/python_motion_planning/common/env/map/grid.py

@@ -309,7 +309,7 @@ def is_expandable(self, point: tuple, src_point: tuple = None) -> bool:
         if not self.within_bounds(point):
             return False
         if src_point is not None:
-            if self._esdf[point] < self._esdf[src_point]:
+            if self._esdf[point] >= self._esdf[src_point]:
                 return True
         return not self.type_map[point] == TYPES.OBSTACLE and not self.type_map[point] == TYPES.INFLATION
 
@@ -369,9 +369,6 @@ def line_of_sight(self, p1: tuple, p2: tuple) -> list:
         Returns:
             points: List of point on the line of sight.
         """
-        if not self.is_expandable(p1, p2) or not self.is_expandable(p2, p1):
-            return []
-
         p1 = np.array(p1)
         p2 = np.array(p2)
 
@@ -423,7 +420,7 @@ def in_collision(self, p1: tuple, p2: tuple) -> bool:
         Returns:
             in_collision: True if the line of sight is in collision, False otherwise.
         """
-        if not self.is_expandable(p1, p2) or not self.is_expandable(p2, p1):
+        if not self.is_expandable(p1) or not self.is_expandable(p2, p1):
             return True
 
         # Corner Case: Start and end points are the same
@@ -449,11 +446,12 @@ def in_collision(self, p1: tuple, p2: tuple) -> bool:
         steps = abs_delta[primary_axis]
         current = p1
 
-        # Check the start point
-        if not self.is_expandable(tuple(current), tuple(current)):
-            return True
+        # # Check the start point
+        # if not self.is_expandable(tuple(current)):
+        #     return True
 
         for _ in range(steps):
+            last_point = current.copy()
             current[primary_axis] += primary_step
 
             # Update the error for the primary dimension
@@ -467,7 +465,7 @@ def in_collision(self, p1: tuple, p2: tuple) -> bool:
                     error[d] -= delta2[primary_axis]
 
             # Check the current point
-            if not self.is_expandable(tuple(current), tuple(current)):
+            if not self.is_expandable(tuple(current), tuple(last_point)):
                 return True
 
         return False
@@ -488,15 +486,19 @@ def inflate_obstacles(self, radius: float = 1.0) -> None:
         Args:
             radius: Radius of the inflation.
         """
+        self.update_esdf()
         for i in range(self.shape[0]):
             for j in range(self.shape[1]):
-                if self.type_map[i, j] == TYPES.OBSTACLE:
-                    for k in range(round(i-radius), round(i+radius+1)):
-                        for l in range(round(j-radius), round(j+radius+1)):
-                            if k < 0 or k >= self.shape[0] or l < 0 or l >= self.shape[1]:
-                                continue
-                            if self.type_map[k, l] == TYPES.FREE and (k - i)**2 + (l - j)**2 <= radius**2:
-                                self.type_map[k, l] = TYPES.INFLATION
+                if self.esdf[i, j] <= radius and self.type_map[i, j] == TYPES.FREE:
+                    self.type_map[i, j] = TYPES.INFLATION
+
+                # if self.type_map[i, j] == TYPES.OBSTACLE:
+                #     for k in range(round(i-radius), round(i+radius+1)):
+                #         for l in range(round(j-radius), round(j+radius+1)):
+                #             if k < 0 or k >= self.shape[0] or l < 0 or l >= self.shape[1]:
+                #                 continue
+                #             if self.type_map[k, l] == TYPES.FREE and (k - i)**2 + (l - j)**2 <= radius**2:
+                #                 self.type_map[k, l] = TYPES.INFLATION
 
     def fill_expands(self, expands: List[Node]) -> None:
         """

src/python_motion_planning/common/visualizer/visualizer.py

@@ -130,6 +130,8 @@ def plot_path(self, path: list, style: str = "-", color: str = "#13ae00", label:
             marker: marker of path
         '''
         path = np.array(path)
+        if len(path.shape) < 2:
+            return
         if path.shape[1] == 2:
             plt.plot(path[:, 0], path[:, 1], style, lw=linewidth, color=color, label=label, marker=marker)
         elif path.shape[1] == 3:

src/python_motion_planning/path_planner/graph_search/a_star.py

@@ -84,7 +84,7 @@ def plan(self) -> Union[list, dict]:
             node = heapq.heappop(OPEN)
 
             # obstacle found
-            if not self.map_.is_expandable(node.current):
+            if not self.map_.is_expandable(node.current, node.parent):
                 continue
 
             # exists in CLOSED list

src/python_motion_planning/path_planner/graph_search/theta_star.py

@@ -47,7 +47,7 @@ def plan(self) -> tuple:
             node = heapq.heappop(OPEN)
 
             # obstacle found
-            if not self.map_.is_expandable(node.current):
+            if not self.map_.is_expandable(node.current, node.parent):
                 continue
 
             # exists in CLOSED list