diff --git a/mpi_sim/components/social_force_navigation.py b/mpi_sim/components/social_force_navigation.py
index e40c842f0ccd25fd1af49c8fa69c7e7a3f9339a0..2023c12c0b9eab008a67ac794124fa544a0d5736 100644
--- a/mpi_sim/components/social_force_navigation.py
+++ b/mpi_sim/components/social_force_navigation.py
@@ -63,7 +63,6 @@ class SocialForceNavigation(GeneratorComponent):
# print('stats : ', self.box2d_body.inertia, self.box2d_body.awake, self.box2d_body.active, self.box2d_body.transform)
# print(self.box2d_body.localCenter, self.box2d_body.worldCenter, self.box2d_body.linearVelocity, self.box2d_body.angularVelocity)
-
# if goal position is given use it to update the human
if self.state.goal_position is not None:
@@ -93,7 +92,6 @@ class SocialForceNavigation(GeneratorComponent):
next_orientation = self.object.orientation + f_orientation * self.config.step_length_coeff * self.simulation.step_length
self.object.orientation = next_orientation
-
# TODO: include manual control if wanted
# if self.state.goal_position is not None or self.state.goal_orientation is not None:
diff --git a/mpi_sim/components/social_mapping.py b/mpi_sim/components/social_mapping.py
index d8cdf697f1d86efaeb18a70614c1146743e8eebb..c5dfbdadfc4c57f5dc5248f991e1b78990eb5280 100644
--- a/mpi_sim/components/social_mapping.py
+++ b/mpi_sim/components/social_mapping.py
@@ -71,6 +71,17 @@ class SocialMapping(SensorComponent):
def local_map_shape(self):
r"""The shape of the local map defined as (width, height)."""
return self._local_map_shape
+
+ # @property
+ # def human_velocities(self):
+ # if self.human_velocities is None:
+ # pass
+ # else :
+ # return self.human_velocities
+
+ # @human_velocities.setter
+ # def human_velocities(self, human_velocities):
+ # self.human_velocities = human_velocities
def set_global_map(self, global_map):
@@ -111,6 +122,8 @@ class SocialMapping(SensorComponent):
resolution=self._resolution
)
+ self.human_velocities = None
+
def _add_to_simulation(self):
@@ -131,7 +144,8 @@ class SocialMapping(SensorComponent):
self.state.global_map = mpi_sim.utils.create_social_map(
self.simulation,
area=self._global_map_area,
- resolution=self._resolution
+ resolution=self._resolution,
+ human_velocities=self.human_velocities
)
return self.state.global_map.map
diff --git a/mpi_sim/utils/social_map.py b/mpi_sim/utils/social_map.py
index b63427a1a92e3af5d596a88f420e7614d0801366..9f8e98b4785d615dd5be3ec1399a9049d54e5412 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):
+def create_social_map(simulation, area=None, resolution=0.05, human_velocities=None):
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.
@@ -130,11 +130,11 @@ def create_social_map(simulation, area=None, resolution=0.05):
)
persons = []
- for bodie in bodies :
+ for id, bodie in enumerate(bodies) :
x, y = np.array(bodie.position)
- theta = bodie.angle
- # TODO : Study why the speed is zero
- v = 2 + np.sqrt(np.array(bodie.linearVelocity)[0]**2 + np.array(bodie.linearVelocity)[1]**2)
+ theta = np.pi/2 + bodie.angle
+ v = human_velocities[id] if human_velocities else 0
+ # 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))
@@ -142,7 +142,7 @@ def create_social_map(simulation, area=None, resolution=0.05):
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 = ssn.calc_eps(x_mesh, y_mesh, persons)
+ 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)
diff --git a/test/components/test_social_mapping.py b/test/components/test_social_mapping.py
new file mode 100644
index 0000000000000000000000000000000000000000..4dd17710721345b47fc201ba088e4a8afd1a44c6
--- /dev/null
+++ b/test/components/test_social_mapping.py
@@ -0,0 +1,284 @@
+
+#TODO
+
+# import mpi_sim
+# import numpy.testing as np_test
+# from Box2D import b2PolygonShape, b2CircleShape
+# import numpy as np
+
+
+# class DummyAgentObject(mpi_sim.Object):
+
+# @staticmethod
+# def default_config():
+# dc = mpi_sim.Object.default_config()
+# dc.position = (0, 0)
+# dc.orientation = 1
+# dc.properties.append('dynamic')
+# return dc
+
+
+# @property
+# def position(self):
+# return self.box2d_body.position
+
+
+# @property
+# def orientation(self):
+# return self.box2d_body.angle
+
+
+# def _create_in_box2d_world(self, box2d_world):
+# self.box2d_body = box2d_world.CreateStaticBody(
+# position=self.config.position,
+# angle=self.config.orientation,
+# shapes=b2CircleShape(radius=.5),
+# )
+# return self.box2d_body
+
+
+# class PolygonObject(mpi_sim.Object):
+
+# @staticmethod
+# def default_config():
+# dc = mpi_sim.Object.default_config()
+# dc.position = (0, 0)
+# dc.width = 1
+# dc.height = 1
+# dc.properties.append('static')
+# return dc
+
+
+# def _create_in_box2d_world(self, box2d_world):
+# return box2d_world.CreateStaticBody(
+# position=self.config.position,
+# shapes=b2PolygonShape(box=(self.config.width, self.config.height)),
+# )
+
+
+# def test_no_rotation():
+
+# # create the simulation
+# sim = mpi_sim.Simulation(visible_area=((0, 10), (0, 5)))
+
+# agent = DummyAgentObject(
+# position=(0, 0),
+# orientation=0,
+# components=[dict(
+# type=mpi_sim.components.OccupancyGridMapping,
+# name='mapping',
+# object_filter=dict(properties='static'),
+# depth=3.,
+# resolution=1.,
+# )]
+# )
+
+# sim.add_object(agent)
+# sim.add_object(PolygonObject(position=(1,1), width=1.5, height=1.5))
+
+# # check default values before doing the mapping
+# expected_global = np.array([
+# [0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
+# [0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
+# [0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
+# [0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
+# [0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
+# ])
+# expected_global = np.flip(expected_global, axis=0).transpose()
+# np_test.assert_array_almost_equal(expected_global, agent.mapping.global_map)
+# assert agent.mapping.global_map_shape == agent.mapping.global_map.shape
+# assert agent.mapping.global_map_area == ((0, 10), (0, 5))
+
+# expected_local = np.array([
+# [0., 0., 0., 0., 0., 0.],
+# [0., 0., 0., 0., 0., 0.],
+# [0., 0., 0., 0., 0., 0.],
+# [0., 0., 0., 0., 0., 0.],
+# [0., 0., 0., 0., 0., 0.],
+# [0., 0., 0., 0., 0., 0.],
+# ])
+# expected_local = np.flip(expected_local, axis=0).transpose()
+# np_test.assert_array_almost_equal(expected_local, agent.mapping.local_map)
+# assert agent.mapping.local_map_shape == agent.mapping.local_map.shape
+# assert agent.mapping.local_map_area == ((-3, 3), (-3, 3))
+
+# #######################################
+
+# # update both maps
+# agent.mapping.update_global_map()
+# agent.mapping.update_local_map()
+
+# expected_global = np.array([
+# [0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
+# [0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
+# [1., 1., 1., 0., 0., 0., 0., 0., 0., 0.],
+# [1., 1., 1., 0., 0., 0., 0., 0., 0., 0.],
+# [1., 1., 1., 0., 0., 0., 0., 0., 0., 0.],
+# ])
+# expected_global = np.flip(expected_global, axis=0).transpose()
+# np_test.assert_array_almost_equal(expected_global, agent.mapping.global_map)
+# assert agent.mapping.global_map_shape == agent.mapping.global_map.shape
+# assert agent.mapping.global_map_area == ((0, 10), (0, 5))
+
+# expected_local = np.array([
+# [0., 0., 0., 1., 1., 1.],
+# [0., 0., 0., 1., 1., 1.],
+# [0., 0., 0., 1., 1., 1.],
+# [0., 0., 0., 0., 0., 0.],
+# [0., 0., 0., 0., 0., 0.],
+# [0., 0., 0., 0., 0., 0.],
+# ])
+# expected_local = np.flip(expected_local, axis=0).transpose()
+# np_test.assert_array_almost_equal(expected_local, agent.mapping.local_map)
+# assert agent.mapping.local_map_shape == agent.mapping.local_map.shape
+# assert agent.mapping.local_map_area == ((-3, 3), (-3, 3))
+
+
+# def test_east_rotation():
+
+# # create the simulation
+# sim = mpi_sim.Simulation(visible_area=((0, 10), (0, 5)))
+
+# agent = DummyAgentObject(
+# position=(0, 0),
+# orientation=-np.pi / 2,
+# components=[dict(
+# type=mpi_sim.components.OccupancyGridMapping,
+# name='mapping',
+# object_filter=dict(properties='static'),
+# depth=3.,
+# resolution=1.,
+# )]
+# )
+
+# sim.add_object(agent)
+# sim.add_object(PolygonObject(position=(1,1), width=1.5, height=1.5))
+
+# # update both maps
+# agent.mapping.update_global_map()
+# agent.mapping.update_local_map()
+
+# expected_global = np.array([
+# [0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
+# [0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
+# [1., 1., 1., 0., 0., 0., 0., 0., 0., 0.],
+# [1., 1., 1., 0., 0., 0., 0., 0., 0., 0.],
+# [1., 1., 1., 0., 0., 0., 0., 0., 0., 0.],
+# ])
+# expected_global = np.flip(expected_global, axis=0).transpose()
+# np_test.assert_array_almost_equal(expected_global, agent.mapping.global_map)
+# assert agent.mapping.global_map_shape == agent.mapping.global_map.shape
+# assert agent.mapping.global_map_area == ((0, 10), (0, 5))
+
+# expected_local = np.array([
+# [0., 1., 1., 1., 0., 0.],
+# [0., 1., 1., 1., 0., 0.],
+# [0., 1., 1., 1., 0., 0.],
+# [0., 0., 0., 0., 0., 0.],
+# [0., 0., 0., 0., 0., 0.],
+# [0., 0., 0., 0., 0., 0.],
+# ])
+# expected_local = np.flip(expected_local, axis=0).transpose()
+# np_test.assert_array_almost_equal(expected_local, agent.mapping.local_map)
+# assert agent.mapping.local_map_shape == agent.mapping.local_map.shape
+# assert agent.mapping.local_map_area == ((-3, 3), (-3, 3))
+
+
+# def test_west_rotation():
+
+# # create the simulation
+# sim = mpi_sim.Simulation(visible_area=((0, 10), (0, 5)))
+
+# agent = DummyAgentObject(
+# position=(0, 0),
+# orientation=np.pi / 2,
+# components=[dict(
+# type=mpi_sim.components.OccupancyGridMapping,
+# name='mapping',
+# object_filter=dict(properties='static'),
+# depth=3.,
+# resolution=1.,
+# )]
+# )
+
+# sim.add_object(agent)
+# sim.add_object(PolygonObject(position=(1,1), width=1.5, height=1.5))
+
+# # update both maps
+# agent.mapping.update_global_map()
+# agent.mapping.update_local_map()
+
+# expected_global = np.array([
+# [0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
+# [0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
+# [1., 1., 1., 0., 0., 0., 0., 0., 0., 0.],
+# [1., 1., 1., 0., 0., 0., 0., 0., 0., 0.],
+# [1., 1., 1., 0., 0., 0., 0., 0., 0., 0.],
+# ])
+# expected_global = np.flip(expected_global, axis=0).transpose()
+# np_test.assert_array_almost_equal(expected_global, agent.mapping.global_map)
+# assert agent.mapping.global_map_shape == agent.mapping.global_map.shape
+# assert agent.mapping.global_map_area == ((0, 10), (0, 5))
+
+# expected_local = np.array([
+# [0., 0., 0., 0., 0., 0.],
+# [0., 0., 0., 0., 0., 0.],
+# [0., 0., 0., 1., 1., 1.],
+# [0., 0., 0., 1., 1., 1.],
+# [0., 0., 0., 1., 1., 1.],
+# [0., 0., 0., 0., 0., 0.],
+# ])
+# expected_local = np.flip(expected_local, axis=0).transpose()
+# np_test.assert_array_almost_equal(expected_local, agent.mapping.local_map)
+# assert agent.mapping.local_map_shape == agent.mapping.local_map.shape
+# assert agent.mapping.local_map_area == ((-3, 3), (-3, 3))
+
+
+# def test_south_rotation():
+
+# # create the simulation
+# sim = mpi_sim.Simulation(visible_area=((0, 10), (0, 5)))
+
+# agent = DummyAgentObject(
+# position=(0, 0),
+# orientation=np.pi,
+# components=[dict(
+# type=mpi_sim.components.OccupancyGridMapping,
+# name='mapping',
+# object_filter=dict(properties='static'),
+# depth=3.,
+# resolution=1.,
+# )]
+# )
+
+# sim.add_object(agent)
+# sim.add_object(PolygonObject(position=(1,1), width=1.5, height=1.5))
+
+# # update both maps
+# agent.mapping.update_global_map()
+# agent.mapping.update_local_map()
+
+# expected_global = np.array([
+# [0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
+# [0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
+# [1., 1., 1., 0., 0., 0., 0., 0., 0., 0.],
+# [1., 1., 1., 0., 0., 0., 0., 0., 0., 0.],
+# [1., 1., 1., 0., 0., 0., 0., 0., 0., 0.],
+# ])
+# expected_global = np.flip(expected_global, axis=0).transpose()
+# np_test.assert_array_almost_equal(expected_global, agent.mapping.global_map)
+# assert agent.mapping.global_map_shape == agent.mapping.global_map.shape
+# assert agent.mapping.global_map_area == ((0, 10), (0, 5))
+
+# expected_local = np.array([
+# [0., 0., 0., 0., 0., 0.],
+# [0., 0., 0., 0., 0., 0.],
+# [0., 1., 1., 1., 0., 0.],
+# [0., 1., 1., 1., 0., 0.],
+# [0., 1., 1., 1., 0., 0.],
+# [0., 0., 0., 0., 0., 0.],
+# ])
+# expected_local = np.flip(expected_local, axis=0).transpose()
+# np_test.assert_array_almost_equal(expected_local, agent.mapping.local_map)
+# assert agent.mapping.local_map_shape == agent.mapping.local_map.shape
+# assert agent.mapping.local_map_area == ((-3, 3), (-3, 3))