From aa2d024e61190a882f2e4fcd4122a73d7830f8c4 Mon Sep 17 00:00:00 2001
From: SANCHEZ Victor <victor.sanchez@inria.fr>
Date: Sat, 17 Feb 2024 11:42:32 +0100
Subject: [PATCH] updating grid mapping with imported map
---
mpi_sim/components/occupancy_grid_mapping.py | 14 +++++++++-----
mpi_sim/objects/ari_robot.py | 7 +++++--
mpi_sim/utils/occupancy_grid_map.py | 11 +++++++----
3 files changed, 21 insertions(+), 11 deletions(-)
diff --git a/mpi_sim/components/occupancy_grid_mapping.py b/mpi_sim/components/occupancy_grid_mapping.py
index 69671fc..aa43774 100644
--- a/mpi_sim/components/occupancy_grid_mapping.py
+++ b/mpi_sim/components/occupancy_grid_mapping.py
@@ -25,6 +25,8 @@ class OccupancyGridMapping(SensorComponent):
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
dc.position_noise = 0
dc.orientation_noise = 0
return dc
@@ -205,10 +207,11 @@ class OccupancyGridMapping(SensorComponent):
self.state.local_map = mpi_sim.utils.create_local_perspective_occupancy_grid_map(
global_map=self.state.global_map,
# 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
+ 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
+ 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
@@ -229,9 +232,10 @@ class OccupancyGridMapping(SensorComponent):
self.state.local_map_noisy = mpi_sim.utils.create_local_perspective_occupancy_grid_map(
global_map=self.state.global_map,
position=self.object.position + self.config.position_noise*np.array([np.random.uniform(low=-1, high=1),np.random.uniform(low=-1, high=1)]),
- orientation=constraint_angle(np.pi/2+self.object.orientation) + self.config.orientation_noise*np.random.uniform(low=-1, high=1),
+ orientation=constraint_angle(self.config.transform_real_to_sim_orientation(self.object.orientation)) + self.config.orientation_noise*np.random.uniform(low=-1, high=1),
depth=self.config.depth,
- from_given_center = self.config.imported_global_map_px_coord_center
+ 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_noisy.map
diff --git a/mpi_sim/objects/ari_robot.py b/mpi_sim/objects/ari_robot.py
index 40e68b0..854461c 100644
--- a/mpi_sim/objects/ari_robot.py
+++ b/mpi_sim/objects/ari_robot.py
@@ -57,6 +57,9 @@ class ARIRobot(Object):
dc.max_movement_force = 0.25
dc.max_rotation_force = 6.
+ dc.linearDamping = 2
+ dc.angularDamping = 2
+
return dc
@@ -262,8 +265,8 @@ class ARIRobot(Object):
)
self.box2d_body.userData = dict(obj=self)
self.box2d_body.userData['name'] = self.config.id
- self.box2d_body.linearDamping = 2
- self.box2d_body.angularDamping = 2
+ self.box2d_body.linearDamping = self.config.linearDamping
+ self.box2d_body.angularDamping = self.config.angularDamping
self.box2d_body.CreatePolygonFixture(
box=(*wheel_dim, b2Vec2(tire_anchors[0]), 0),
diff --git a/mpi_sim/utils/occupancy_grid_map.py b/mpi_sim/utils/occupancy_grid_map.py
index 67a5870..789dbb2 100644
--- a/mpi_sim/utils/occupancy_grid_map.py
+++ b/mpi_sim/utils/occupancy_grid_map.py
@@ -180,7 +180,7 @@ def create_occupancy_grid_map(simulation, area=None, resolution=0.05, object_fil
return map
-def create_local_perspective_occupancy_grid_map(global_map, depth=5.0, position=(0,0), orientation=0.0, from_given_center=None):
+def create_local_perspective_occupancy_grid_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 occupancy grid map.
Args:
@@ -209,9 +209,12 @@ def create_local_perspective_occupancy_grid_map(global_map, depth=5.0, position=
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)
--
GitLab