Merge branch 'feature/fov' into dev

changelog.md

@@ -1,9 +1,10 @@
 ## ir-sim 2.3.1
 
-- Add FOV for the object (doc require)
+- Add FOV for the object (doc require), see usage 16fov_world for detail
 - Add Arguments for function WrapToPi
 - Add the role selection for the rvo behavior (doc require)
-
+- Update Documentation (Fov, noise world, path manager)
+- Fix some bugs
 
 ## ir-sim 2.3.0
 

irsim/env/env_base.py

@@ -25,10 +25,12 @@
 
 try:
     from pynput import keyboard
+
     keyboard_module = True
 except ImportError:
     keyboard_module = False
 
+
 class EnvBase:
     """
     The base class of environment. This class will read the yaml file and create the world, robot, obstacle, and map objects.
@@ -118,7 +120,7 @@ def __del__(self):
     def __str__(self):
         return f"Environment: {self._world.name}"
 
-    def step(self, action=None, action_id=0):
+    def step(self, action: Optional[np.ndarray] = None, action_id=0):
         """
         Perform a simulation step in the environment.
 
@@ -142,16 +144,16 @@ def step(self, action=None, action_id=0):
 
         self._world.step()
 
-    def _objects_step(self, action=None):
+    def _objects_step(self, action: Optional[list] =None):
         action = action + [None] * (len(self.objects) - len(action))
         [obj.step(action) for obj, action in zip(self.objects, action)]
 
-    def _object_step(self, action, obj_id=0):
+    def _object_step(self, action: np.ndarray, obj_id: int = 0):
         self.objects[obj_id].step(action)
         [obj.step() for obj in self.objects if obj._id != obj_id]
 
     # render
-    def render(self, interval=0.05, figure_kwargs=dict(), **kwargs):
+    def render(self, interval: float =0.05, figure_kwargs=dict(), **kwargs):
         """
         Render the environment.
 
@@ -282,7 +284,7 @@ def init_keyboard(self, keyboard_kwargs=dict()):
             ["r", "reset the environment"],
         ]
         # headers = ["key", "function"]
-        
+
         headers = ["Key", "Function"]
         # Generate the table using tabulate
         table = tabulate(commands, headers=headers, tablefmt="grid")

irsim/lib/algorithm/generation.py

@@ -82,7 +82,7 @@ def generate_polygon(center, avg_radius, irregularity, spikeyness, num_vertices)
     angle_steps = random_angle_steps(num_vertices, irregularity)
 
     points = []
-    angle = random.uniform(0, 2 * math.pi)
+    angle = np.random.uniform(0, 2 * math.pi)
     for i in range(num_vertices):
         radius = clip(random.gauss(avg_radius, spikeyness), 0, 2 * avg_radius)
         point = (
@@ -113,7 +113,7 @@ def random_angle_steps(steps: int, irregularity: float) -> List[float]:
     upper = (2 * math.pi / steps) + irregularity
     cumsum = 0
     for i in range(steps):
-        angle = random.uniform(lower, upper)
+        angle = np.random.uniform(lower, upper)
         angles.append(angle)
         cumsum += angle
 

irsim/usage/16fov_world/fov_world.py

@@ -1,12 +1,18 @@
 import irsim
+from irsim.global_param.path_param import path_manager
 
-env = irsim.make(save_ani=False)
+path_manager.ani_path = path_manager.root_path + '/usage/16fov_world/test_123'
+path_manager.ani_buffer_path = path_manager.root_path + '/usage/16fov_world/test_123_buffer'
+full_path = path_manager.root_path + '/usage/16fov_world/fov_world.yaml'
 
-for i in range(3000):
+env = irsim.make(full_path, save_ani=True)
+
+for i in range(10):
 
     env.step()
     # env.robot.set_state([x, y, theta])       
-
+    # env.robot.set_goal([10, 30, 0])
+
     for obs in env.obstacle_list:
         if obs.fov_detect_object(env.robot):
             print(f'The robot is in the FOV of the {obs.name}. The parameters of this obstacle are: state [x, y, theta]: {obs.state.flatten()}, velocity [linear, angular]: {obs.velocity.flatten()}, fov in radian: {obs.fov}.')

irsim/usage/16fov_world/fov_world.yaml

@@ -13,11 +13,12 @@ robot:
     shape: {name: 'circle', radius: 0.4}
     state: [5, 5, 0, 0]
     goal: [40, 40, 0]
+    goal_threshold: 0.4
     vel_max: [3, 1]
     vel_min: [-3, -1]
-    behavior: {name: 'dash'} # move toward to the goal directly
+    behavior: {name: 'dash', wander: True, range_low: [15, 15, -3.14], range_high: [35, 35, 3.14]} 
     plot:
-        show_goal: False
+        show_goal: True
         show_trajectory: True
 
 obstacle:

irsim/world/object_base.py

@@ -669,6 +669,37 @@ def set_init_geometry(self, geometry):
         """
         self._init_geometry = geometry
 
+    def set_goal(self, goal=[10, 10, 0]):
+        '''
+        Set the goal of the object.
+
+        Args:
+            goal: The goal of the object [x, y, theta].
+        '''
+        if isinstance(goal, list):
+            if len(goal) > self.state_dim:
+                temp_goal = np.c_[goal[: self.state_dim]]
+            elif len(goal) < self.state_dim:
+                temp_goal = np.c_[goal + [0] * (self.state_dim - len(goal))]
+            else:
+                temp_goal = np.c_[goal]
+
+        elif isinstance(goal, np.ndarray):
+            if goal.shape[0] > self.state_dim:
+                temp_goal = goal[: self.state_dim]
+            elif goal.shape[0] < self.state_dim:
+                temp_goal = np.r_[
+                    goal, np.zeros((self.state_dim - goal.shape[0], goal.shape[1]))
+                ]
+            else:
+                temp_goal = goal
+
+        assert self._goal.shape == temp_goal.shape
+
+        self._goal = temp_goal.copy()
+
+
+
     def geometry_state_transition(self):
         pass
 

irsim/world/world.py

@@ -3,7 +3,7 @@
 import numpy as np
 import matplotlib.image as mpimg
 from typing import Optional
-
+import os
 
 class World:
     """
@@ -50,7 +50,8 @@ def __init__(
             obstacle_map: Image file for the obstacle map.
             mdownsample (int): Downsampling factor for the obstacle map.
         """
-        self.name = name.split('.')[0]
+
+        self.name = os.path.basename(name).split('.')[0]
         self.height = height
         self.width = width
         self.step_time = step_time

todo_list.md

@@ -66,4 +66,7 @@
 - [ ] robotics arm support
 - [ ] assign robot goals by mouse click
 - [ ] Add wrapper for ORCA algorithm
-- [ ] Reorganize the structure of the readme demonstration
+- [ ] Reorganize the structure of the readme demonstration
+- [ ] argument type hint
+- [ ] Doc Noise world
+- [ ] Doc path manager and change the path