From ed8c53344f892479317311a81dab4d7980f0eee5 Mon Sep 17 00:00:00 2001
From: SANCHEZ Victor <victor.sanchez@inria.fr>
Date: Mon, 12 Feb 2024 15:34:10 +0100
Subject: [PATCH] updates
---
mpi_sim/components/occupancy_grid_mapping.py | 5 +++--
mpi_sim/utils/social_map.py | 18 +++++++-----------
2 files changed, 10 insertions(+), 13 deletions(-)
diff --git a/mpi_sim/components/occupancy_grid_mapping.py b/mpi_sim/components/occupancy_grid_mapping.py
index 5b59d24..69671fc 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 9f8e98b..81bdb81 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))
--
GitLab