Add feature of checking the convex and CCW obstacles

.gitignore

@@ -127,4 +127,5 @@ dmypy.json
 
 # Pyre type checker
 .pyre/
-.vscode/
+.vscode/
+test.py

RDA_planner/mpc.py

@@ -414,6 +414,12 @@ def convert_inequal_polygon(self, vertex, velocity=np.zeros((2, 1))):
 
     def gen_inequal_global(self, vertex):
 
+        convex_flag, order = self.is_convex_and_ordered(vertex)
+
+        assert convex_flag, 'The polygon constructed by vertex is not convex. Please check the vertex.'
+
+        if order == 'CW': vertex = vertex[:, ::-1]
+
         temp_vertex = np.c_[vertex, vertex[0:2, 0]]   
 
         point_num = vertex.shape[1]
@@ -438,4 +444,43 @@ def gen_inequal_global(self, vertex):
         return A, b 
 
 
+    def cross_product(self, o, a, b):
+        """Compute the cross product of vectors OA and OB.
+        A positive cross product indicates a counter-clockwise turn,
+        a negative indicates a clockwise turn, and zero indicates a collinear point."""
+        return (a[0] - o[0]) * (b[1] - o[1]) - (a[1] - o[1]) * (b[0] - o[0])
+
+    def is_convex_and_ordered(self, points):
+        """Determine if the polygon is convex and return the order (CW or CCW).
+        
+        Args:
+            points (np.ndarray): A 2xN NumPy array representing the vertices of the polygon.
+            
+        Returns:
+            (bool, str): A tuple where the first element is True if the polygon is convex,
+                        and the second element is 'CW' or 'CCW' based on the order.
+                        If not convex, returns (False, None).
+        """
+        n = points.shape[1]  # Number of points
+        if n < 3:
+            return False, None  # A polygon must have at least 3 points
+
+        # Initialize the direction for the first cross product
+        direction = 0
+
+        for i in range(n):
+            o = points[:, i]
+            a = points[:, (i + 1) % n]
+            b = points[:, (i + 2) % n]
+
+            cross = self.cross_product(o, a, b)
+
+            if cross != 0:  # Only consider non-collinear points
+                if direction == 0:
+                    direction = 1 if cross > 0 else -1
+                elif (cross > 0 and direction < 0) or (cross < 0 and direction > 0):
+                    return False, None  # Not convex
+
+        return True, 'CCW' if direction > 0 else 'CW'
+
 

README.md

@@ -48,7 +48,7 @@ pip install -e .
 **Dynamic obstacles avoidance (example/dynamic_obs.py)** | <img src="example\dynamic_obs\animation\dynamic_obs1.gif" width="300" /> |  <img src="example\dynamic_obs\animation\dynamic_obs2.gif" width="300" />
 |:-------------------------:|:-------------------------:|:-------------------------:|
 
-**Note:** You can customize the scenario by modifying the parameters in the corresponding yaml file as introduced in [ir_sim](https://github.com/hanruihua/ir_sim). For the polygon obstacles, please make sure the order of the vertices is Counterclockwise.
+**Note:** You can customize the scenario by modifying the parameters in the corresponding yaml file as introduced in [ir_sim](https://github.com/hanruihua/ir_sim). For the polygon obstacles, please make sure the obstacles are convex (CCW order is not necessary now).
 
 
 ## Citation

example/path_track/path_track.yaml

@@ -31,7 +31,7 @@ obstacle:
   - number: 1
     distribution: {name: 'manual'}
     shape:
-      - {name: 'polygon', vertices: [[31, 24], [33, 24], [33, 28], [31, 28]]}
+      - {name: 'polygon', vertices: [[31, 28], [33, 28], [33, 24], [31, 24]]}
     state: 
       - [0, 0, 0] 
 

example/path_track/path_track_diff.py

@@ -40,7 +40,7 @@ def main():
             print('arrive at the goal')
             break
 
-    env.end(ani_name='path_track', show_traj=True, show_trail=True, ending_time=10, ani_kwargs={'subrectangles':True})
+    env.end(ani_name='path_track_diff', show_traj=True, show_trail=True, ending_time=10)
 
 if __name__ == '__main__':
     main()

setup.py

@@ -4,12 +4,12 @@
 setup(
     name='RDA_planner',
     py_modules=['RDA_planner'],
-    version= '2.1',
+    version= '2.2',
     install_requires=[
         'cvxpy==1.5.2',
         'numpy',
         'pathos',
-        'ir_sim==2.1.1',
+        'ir_sim==2.1.2',
         'matplotlib',
         'gctl==1.1',
         'opencv-python',