diff --git a/mpi_sim/components/occupancy_grid_mapping.py b/mpi_sim/components/occupancy_grid_mapping.py index aa43774bad537e3f487039c8e29bb25fa3d3b03b..4b1620d78d4a1868c1786f5f9b5fb9c1c9286a5b 100644 --- a/mpi_sim/components/occupancy_grid_mapping.py +++ b/mpi_sim/components/occupancy_grid_mapping.py @@ -125,15 +125,14 @@ class OccupancyGridMapping(SensorComponent): if isinstance(self.config.imported_global_map_path, str): - imported_global_map = np.load(file=self.config.imported_global_map_path) + self.imported_global_map = np.load(file=self.config.imported_global_map_path) self.state.global_map = mpi_sim.utils.OccupancyGridMap( - map=imported_global_map, + map=self.imported_global_map, area=self._global_map_area, resolution=self._resolution ) - self._global_map_shape = np.shape(imported_global_map) else : raise UserWarning('The parameter \'imported_global_map_path\' should be a type str but it is {}'.format(type(self.config.imported_global_map_path))) @@ -177,15 +176,15 @@ class OccupancyGridMapping(SensorComponent): transform=self.config.transform ) else : - imported_global_map = np.load(file=self.config.imported_global_map_path) + #imported_global_map = cv2.circle(imported_global_map*255, center = [75, 65], radius = 7, color = (255, 255, 255), thickness=-1)/255 if self.config.transform: - imported_global_map=self.config.transform(imported_global_map) + self.imported_global_map=self.config.transform(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 - ) + map=self.imported_global_map, + area=self._global_map_area, + resolution=self._resolution + ) return self.state.global_map.map diff --git a/mpi_sim/components/social_mapping.py b/mpi_sim/components/social_mapping.py index 5c01f30b9fef985cf29007196c1d193e618894ec..f795c7a3330650cb2579f2640ea7dbef3b0cef56 100644 --- a/mpi_sim/components/social_mapping.py +++ b/mpi_sim/components/social_mapping.py @@ -22,6 +22,12 @@ class SocialMapping(SensorComponent): dc.resolution = 0.05 # resolution of the maps in m per cell dc.update_global_map_automatically = True # should the global map be automatically updated each step dc.update_local_map_automatically = False # update local map each time the smulation is step + 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 + dc.transform_position_to_map_coordinate = None + dc.transform_real_to_sim_orientation = None return dc @@ -87,26 +93,50 @@ class SocialMapping(SensorComponent): def __init__(self, config=None, **kwargs): super().__init__(config=config, **kwargs) + self.state.global_map = None - - self._resolution = self.config.resolution - self._global_map_area = self.config.area + if not self.config.imported_global_map_path : + self._resolution = self.config.resolution + self._global_map_area = self.config.area - self._global_map_shape = None - self.state.global_map = None + self._global_map_shape = None + + # is the global map initially created, if not do it before creating the local map + self.state.is_global_map_initialized = False - # is the global map initially created, if not do it before creating the local map - self.state.is_global_map_initialized = False + # initialize the global map if possible + if self._global_map_area is not None: + self._global_map_shape = mpi_sim.utils.get_social_map_shape(area=self._global_map_area, resolution=self._resolution) + self.state.global_map = mpi_sim.utils.SocialMap( + map=np.zeros(self._global_map_shape), + area=self._global_map_area, + resolution=self._resolution + ) - # initialize the global map if possible - if self._global_map_area is not None: - self._global_map_shape = mpi_sim.utils.get_social_map_shape(area=self._global_map_area, resolution=self._resolution) - self.state.global_map = mpi_sim.utils.SocialMap( - map=np.zeros(self._global_map_shape), - area=self._global_map_area, - resolution=self._resolution - ) + + + + else : + self._resolution = self.config.imported_global_map_resolution + 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.imported_global_map_path, str): + + imported_global_map = np.load(file=self.config.imported_global_map_path) + self.shape_imported_global_map = np.shape(imported_global_map) + self.state.global_map = mpi_sim.utils.SocialMap( + map=np.zeros_like(imported_global_map), + area=self._global_map_area, + resolution=self._resolution + ) + + self._global_map_shape = np.shape(imported_global_map) + else : + 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.human_velocities = None 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) self._local_map_shape = (local_map_length, local_map_length) @@ -115,10 +145,7 @@ class SocialMapping(SensorComponent): area=None, resolution=self._resolution ) - - self.human_velocities = None - - + def _add_to_simulation(self): # if the global map area should span the whole simulation (config.area=None), then read visible_area from the simulation when added to it @@ -134,13 +161,22 @@ class SocialMapping(SensorComponent): def update_global_map(self): - - self.state.global_map = mpi_sim.utils.create_social_map( - self.simulation, - area=self._global_map_area, - resolution=self._resolution, - human_velocities=self.human_velocities - ) + if not self.config.imported_global_map_path : + + self.state.global_map = mpi_sim.utils.create_social_map( + self.simulation, + area=self._global_map_area, + resolution=self._resolution, + human_velocities=self.human_velocities + ) + else : + self.state.global_map = mpi_sim.utils.create_social_map( + self.simulation, + area=self._global_map_area, + resolution=self._resolution, + human_velocities=self.human_velocities, + flag_flip=True + ) return self.state.global_map.map @@ -148,13 +184,24 @@ class SocialMapping(SensorComponent): def update_local_map(self): if self.state.global_map is None: self.update_global_map() - self.state.local_map = mpi_sim.utils.create_local_perspective_social_map( - global_map=self.state.global_map, - position=self.object.position, - orientation=constraint_angle(self.object.orientation), - depth=self.config.depth, - ) - + if not self.config.imported_global_map_path : + self.state.local_map = mpi_sim.utils.create_local_perspective_social_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 + self.state.local_map = mpi_sim.utils.create_local_perspective_social_map( + global_map=self.state.global_map, + # find a way to explain this + position=self.object.position, + orientation=constraint_angle(self.config.transform_real_to_sim_orientation(self.object.orientation)), + depth=self.config.depth, + from_given_center = self.config.imported_global_map_px_coord_center, + transform_position_to_map_coordinate = self.config.transform_position_to_map_coordinate + ) return self.state.local_map.map diff --git a/mpi_sim/utils/social_map.py b/mpi_sim/utils/social_map.py index 81bdb8108a01b5fcb4e87e14dd2561522d608ab5..626eb1dc909f551f303efe3d68068023247d099e 100644 --- a/mpi_sim/utils/social_map.py +++ b/mpi_sim/utils/social_map.py @@ -83,7 +83,7 @@ def transform_map_coordinate_to_position(map_coordinate, map_area, map_resolutio return x, y -def create_social_map(simulation, area=None, resolution=0.05, human_velocities=None): +def create_social_map(simulation, area=None, resolution=0.05, human_velocities=None, flag_flip=False): r"""Creates an social map. The size of the map depends on the visible_map_area of the simulation and the resolution of the map. @@ -108,10 +108,11 @@ def create_social_map(simulation, area=None, resolution=0.05, human_velocities=N # visible_area: ((min_x, max_x), (min_y, max_y)) x_width = area[0][1] - area[0][0] y_width = area[1][1] - area[1][0] - width = x_width / resolution height = y_width / resolution + + object_filter = dict(types=mpi_sim.objects.Human) bodies = simulation.box2d_simulation.get_bodies( @@ -130,7 +131,6 @@ def create_social_map(simulation, area=None, resolution=0.05, human_velocities=N vx = human_velocities[id][0] vy = human_velocities[id][1] v = np.sqrt(vx**2 + vy**2) - # TODO : Check that the order of the bodies is the same : i.e. that the added speeds actually match persons.append((x ,y, theta, v, False)) @@ -138,14 +138,17 @@ def create_social_map(simulation, area=None, resolution=0.05, human_velocities=N y_coordinates = np.linspace(area[1][1], area[1][0], int(height)) x_mesh, y_mesh = np.meshgrid(x_coordinates, y_coordinates) - global_social_map = np.flip(np.flip(ssn.calc_eps(x_mesh, y_mesh, persons), axis=1), axis=0).T + if not flag_flip: + global_social_map = np.flip(np.flip(ssn.calc_eps(x_mesh, y_mesh, persons), axis=1), axis=0).T + else : + global_social_map = np.flip(np.flip(ssn.calc_eps(x_mesh, y_mesh, persons), axis=1), axis=0).T map = SocialMap(map=global_social_map, area=area, resolution=resolution) return map -def create_local_perspective_social_map(global_map, depth=5.0, position=(0,0), orientation=0.0, from_given_center=None): +def create_local_perspective_social_map(global_map, depth=5.0, position=(0,0), orientation=0.0, from_given_center=None, transform_position_to_map_coordinate=transform_position_to_map_coordinate): r"""Creates a local perspective of an social map. Args: @@ -174,9 +177,11 @@ def create_local_perspective_social_map(global_map, depth=5.0, position=(0,0), o map_resolution=global_map.resolution ) else : - center_x, center_y = from_given_center - position_px = position/global_map.resolution - pos_in_map = np.array([center_x - np.array(position_px)[1], center_y + np.array(position_px)[0]]) + pos_in_map = transform_position_to_map_coordinate( + center_position=from_given_center, + position=position, + map_resolution=global_map.resolution, + ) orientation = mpi_sim.utils.constraint_angle(orientation)