Mentions légales du service

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

social mapping adapted to map imported

parent 4d628460
No related branches found
No related tags found
No related merge requests found
......@@ -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
......
......@@ -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
......
......@@ -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)
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment