Mentions légales du service
Skip to content
GitLab
Explore
Sign in
Primary navigation
Search or go to…
Project
M
multiparty_interaction_simulator
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Code
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Deploy
Releases
Package registry
Model registry
Operate
Terraform modules
Monitor
Service Desk
Analyze
Value stream analytics
Contributor analytics
Repository analytics
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
Community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
Admin message
GitLab upgrade completed. Current version is 17.11.4.
Show more breadcrumbs
Spring
WP6_Robot_Behavior
multiparty_interaction_simulator
Commits
d79506b8
Commit
d79506b8
authored
1 year ago
by
SANCHEZ Victor
Browse files
Options
Downloads
Patches
Plain Diff
social mapping adapted to map imported
parent
4d628460
No related branches found
No related tags found
No related merge requests found
Changes
3
Hide whitespace changes
Inline
Side-by-side
Showing
3 changed files
mpi_sim/components/occupancy_grid_mapping.py
+8
-9
8 additions, 9 deletions
mpi_sim/components/occupancy_grid_mapping.py
mpi_sim/components/social_mapping.py
+80
-33
80 additions, 33 deletions
mpi_sim/components/social_mapping.py
mpi_sim/utils/social_map.py
+13
-8
13 additions, 8 deletions
mpi_sim/utils/social_map.py
with
101 additions
and
50 deletions
mpi_sim/components/occupancy_grid_mapping.py
+
8
−
9
View file @
d79506b8
...
...
@@ -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
...
...
This diff is collapsed.
Click to expand it.
mpi_sim/components/social_mapping.py
+
80
−
33
View file @
d79506b8
...
...
@@ -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
...
...
This diff is collapsed.
Click to expand it.
mpi_sim/utils/social_map.py
+
13
−
8
View file @
d79506b8
...
...
@@ -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
)
...
...
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Save comment
Cancel
Please
register
or
sign in
to comment