diff --git a/mpi_sim/components/occupancy_grid_mapping.py b/mpi_sim/components/occupancy_grid_mapping.py index 5b59d247e0c9b8f29ed4f5a1b833fea954931d72..69671fcc0cbe76e5201308f8f17226651a283355 100644 --- a/mpi_sim/components/occupancy_grid_mapping.py +++ b/mpi_sim/components/occupancy_grid_mapping.py @@ -204,8 +204,9 @@ class OccupancyGridMapping(SensorComponent): # 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_occupancy_grid_map( global_map=self.state.global_map, - position=self.object.position, - orientation=constraint_angle(np.pi/2+self.object.orientation), + # find a way to explain this + position=self.object.position, # @ np.array([[0, 1],[-1, 0]]), + orientation=constraint_angle(np.pi/2+self.object.orientation), # TODO : define a function that adjust the angle depending on the map depth=self.config.depth, from_given_center = self.config.imported_global_map_px_coord_center ) diff --git a/mpi_sim/utils/social_map.py b/mpi_sim/utils/social_map.py index 9f8e98b4785d615dd5be3ec1399a9049d54e5412..81bdb8108a01b5fcb4e87e14dd2561522d608ab5 100644 --- a/mpi_sim/utils/social_map.py +++ b/mpi_sim/utils/social_map.py @@ -94,16 +94,7 @@ def create_social_map(simulation, area=None, resolution=0.05, human_velocities=N area (tuple): Area of the simulation that should be mapped in ((x_min, x_max), (y_min, y_max)). (Default: None) If None, it uses the simulation.visible_area. resolution (float): Resolution in meter per cell (Default: 0.05). - object_filter: Defines which objects should be on the map. (Default: None) - If None, then all objects. - Otherwise, a dictionary that can have the following fields: properties, ids, types - properties: List of object properties (strings). See Object.properties. - ids: List of object ids (ints). - types: List of object types (classes). - operator: Either 'or' or 'and'. Defines how the different filters are combined. (Default: 'or') - All objects that fulfill these requirements are added. - default_occupancy_value (float): Occupancy value of objects (Default: 1.0) - occupancy_value_depth (integer): Number of bits used to encode the occupancy value (Default: 32). + human_velocities (list) : List of [x,y] velocities of humans Returns: social map in form of a SocialMap (namedtuple with map, area, and resolution). @@ -133,7 +124,12 @@ def create_social_map(simulation, area=None, resolution=0.05, human_velocities=N for id, bodie in enumerate(bodies) : x, y = np.array(bodie.position) theta = np.pi/2 + bodie.angle - v = human_velocities[id] if human_velocities else 0 + vx,vy = 0,0 + if human_velocities is not None: + if len(human_velocities) > 0 : + 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))