Mentions légales du service

Skip to content
Snippets Groups Projects
Commit 193720d9 authored by SANCHEZ Victor's avatar SANCHEZ Victor
Browse files

adding noisy map in occupanct grid mapping

parent 7495db24
No related branches found
No related tags found
No related merge requests found
...@@ -25,6 +25,8 @@ class OccupancyGridMapping(SensorComponent): ...@@ -25,6 +25,8 @@ class OccupancyGridMapping(SensorComponent):
dc.imported_global_map_resolution = False # Import Global map - resolution dc.imported_global_map_resolution = False # Import Global map - resolution
dc.imported_global_map_area = False # Import Global map - area (can either be a tuple of a function is the area is not squared) dc.imported_global_map_area = False # Import Global map - area (can either be a tuple of a function is the area is not squared)
dc.imported_global_map_px_coord_center = None dc.imported_global_map_px_coord_center = None
dc.position_noise = 0
dc.orientation_noise = 0
return dc return dc
...@@ -62,6 +64,14 @@ class OccupancyGridMapping(SensorComponent): ...@@ -62,6 +64,14 @@ class OccupancyGridMapping(SensorComponent):
return None return None
else: else:
return self.update_local_map() return self.update_local_map()
@property
def local_map_noisy(self):
r"""Get the noisy version of the local occupancy grid map that is centered and oriented around the object of the component."""
if self.state.local_map_noisy is None:
return None
else:
return self.update_noisy_local_map()
@property @property
...@@ -135,6 +145,12 @@ class OccupancyGridMapping(SensorComponent): ...@@ -135,6 +145,12 @@ class OccupancyGridMapping(SensorComponent):
resolution=self._resolution resolution=self._resolution
) )
self.state.local_map_noisy = mpi_sim.utils.OccupancyGridMap(
map=np.zeros(self._local_map_shape),
area=None,
resolution=self._resolution
)
def _add_to_simulation(self): def _add_to_simulation(self):
...@@ -195,6 +211,29 @@ class OccupancyGridMapping(SensorComponent): ...@@ -195,6 +211,29 @@ class OccupancyGridMapping(SensorComponent):
) )
return self.state.local_map.map return self.state.local_map.map
def update_noisy_local_map(self):
if self.state.global_map is None:
self.update_global_map()
if not self.config.imported_global_map_path :
self.state.local_map_noisy = mpi_sim.utils.create_local_perspective_occupancy_grid_map(
global_map=self.state.global_map,
position=self.object.position + self.config.position_noise*np.array([np.random.uniform(low=-1, high=1),np.random.uniform(low=-1, high=1)]),
orientation=constraint_angle(self.object.orientation)+ self.config.orientation_noise*np.random.uniform(low=-1, high=1),
depth=self.config.depth,
)
else :
# Same but we need to relocate the position from the center of the imported room
self.state.local_map_noisy = mpi_sim.utils.create_local_perspective_occupancy_grid_map(
global_map=self.state.global_map,
position=self.object.position + self.config.position_noise*np.array([np.random.uniform(low=-1, high=1),np.random.uniform(low=-1, high=1)]),
orientation=constraint_angle(np.pi/2+self.object.orientation) + self.config.orientation_noise*np.random.uniform(low=-1, high=1),
depth=self.config.depth,
from_given_center = self.config.imported_global_map_px_coord_center
)
return self.state.local_map_noisy.map
def _step(self): def _step(self):
...@@ -203,3 +242,4 @@ class OccupancyGridMapping(SensorComponent): ...@@ -203,3 +242,4 @@ class OccupancyGridMapping(SensorComponent):
self.state.is_global_map_initialized = True self.state.is_global_map_initialized = True
if self.config.update_local_map_automatically : if self.config.update_local_map_automatically :
self.update_local_map() self.update_local_map()
self.update_noisy_local_map()
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment