diff --git a/mpi_sim/components/occupany_grid_mapping.py b/mpi_sim/components/occupany_grid_mapping.py index 8fceb68d54b1aa2a87ff2155de63af497760ecb7..3a2b4587a7178d968e8acea555a36f3a5ecf357c 100644 --- a/mpi_sim/components/occupany_grid_mapping.py +++ b/mpi_sim/components/occupany_grid_mapping.py @@ -24,6 +24,7 @@ class OccupancyGridMapping(SensorComponent): dc.imported_global_map_path = False # Import Global map - path 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_px_coord_center = None return dc @@ -103,7 +104,7 @@ class OccupancyGridMapping(SensorComponent): self._global_map_area = self.config.imported_global_map_area self.state.is_global_map_initialized = True #The global map is initialized and doesn't need to be updated - if isinstance(self.config.import_global_map, str): + if isinstance(self.config.imported_global_map_path, str): imported_global_map = np.load(file=self.config.imported_global_map_path) self.state.global_map = mpi_sim.utils.OccupancyGridMap( @@ -115,7 +116,7 @@ class OccupancyGridMapping(SensorComponent): self._global_map_shape = np.shape(imported_global_map) else : - raise UserWarning('The parameter \'import_global_map\' should be a type str but it is {}'.format(type(self.config.import_global_map))) + raise UserWarning('The parameter \'imported_global_map_path\' should be a type str but it is {}'.format(type(self.config.imported_global_map_path))) self._local_map_area = ((-self.config.depth, self.config.depth), (-self.config.depth, self.config.depth)) local_map_length = int((self.config.depth / self._resolution) * 2) @@ -144,28 +145,49 @@ class OccupancyGridMapping(SensorComponent): def update_global_map(self): - self.state.global_map = mpi_sim.utils.create_occupancy_grid_map( - self.simulation, - area=self._global_map_area, - resolution=self._resolution, - object_filter=self.config.object_filter, - transform=self.config.transform - ) + if not self.config.imported_global_map_path : + self.state.global_map = mpi_sim.utils.create_occupancy_grid_map( + self.simulation, + area=self._global_map_area, + resolution=self._resolution, + object_filter=self.config.object_filter, + transform=self.config.transform + ) + else : + imported_global_map = np.load(file=self.config.imported_global_map_path) + self.state.global_map = mpi_sim.utils.OccupancyGridMap( + map=imported_global_map, + area=self._global_map_area, + resolution=self._resolution + ) return self.state.global_map.map def update_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 = mpi_sim.utils.create_local_perspective_occupancy_grid_map( - global_map=self.state.global_map, - position=self.object.position, - orientation=constraint_angle(self.object.orientation), - depth=self.config.depth, - ) + self.state.local_map = mpi_sim.utils.create_local_perspective_occupancy_grid_map( + global_map=self.state.global_map, + position=self.object.position, + orientation=constraint_angle(self.object.orientation), + depth=self.config.depth, + ) + else : + # Same but we need to relocate the position from the center of the imported room + coord_of_imported_map_center = mpi_sim.utils.transform_map_coordinate_to_position( + map_coordinate=self.config.imported_global_map_px_coord_center, + map_area=self.config.imported_global_map_area, + map_resolution=self.config.imported_global_map_resolution + ) + self.state.local_map = mpi_sim.utils.create_local_perspective_occupancy_grid_map( + global_map=self.state.global_map, + position=self.object.position + coord_of_imported_map_center, + orientation=constraint_angle(self.object.orientation), + depth=self.config.depth, + ) return self.state.local_map.map