From 3959ba01807cf4059aa9ba93f2672dd713ca9d80 Mon Sep 17 00:00:00 2001 From: SANCHEZ Victor <victor.sanchez@inria.fr> Date: Thu, 12 Oct 2023 14:40:53 +0200 Subject: [PATCH] adding function to get object from simulation and new function to measure distance between an object and a position --- mpi_sim/components/occupany_grid_mapping.py | 2 +- mpi_sim/core/simulation.py | 15 +++++ mpi_sim/utils/measurements.py | 68 ++++++++++++++------- 3 files changed, 62 insertions(+), 23 deletions(-) diff --git a/mpi_sim/components/occupany_grid_mapping.py b/mpi_sim/components/occupany_grid_mapping.py index 419f91e..3d18bf4 100644 --- a/mpi_sim/components/occupany_grid_mapping.py +++ b/mpi_sim/components/occupany_grid_mapping.py @@ -151,7 +151,7 @@ class OccupancyGridMapping(SensorComponent): transform=self.config.transform ) else : - imported_global_map = np.load(file=self.config.imported_global_map_path) + imported_global_map = np.load(file=self.config.imported_global_map_path) if self.config.transform: imported_global_map=self.config.transform(imported_global_map) self.state.global_map = mpi_sim.utils.OccupancyGridMap( diff --git a/mpi_sim/core/simulation.py b/mpi_sim/core/simulation.py index 68d49bd..43f7140 100644 --- a/mpi_sim/core/simulation.py +++ b/mpi_sim/core/simulation.py @@ -814,6 +814,21 @@ class Simulation: # box2d simulation self._box2d_simulation.close() + def get_objects_in_simulation(self, object_to_avoid=None): + """Returns the objects in the simulation""" + objects = [] + # go through objects + for obj in self.objects.values(): + if object_to_avoid is None : + # only include objects that have a position property + if obj.position is not None: + objects.append(obj) + else : + if type(obj) != type(object_to_avoid): + if obj.position is not None: + objects.append(obj) + return objects + def get_nearby_objects(self, point, radius, object_types=None): """Returns objects that are within a specified radius of the given point (x,y) or object (obj.position). diff --git a/mpi_sim/utils/measurements.py b/mpi_sim/utils/measurements.py index aca201a..f41ccba 100644 --- a/mpi_sim/utils/measurements.py +++ b/mpi_sim/utils/measurements.py @@ -2,6 +2,7 @@ import numpy as np import mpi_sim import collections import Box2D +from Box2D import b2CircleShape def measure_center_distance(p1, p2): @@ -23,7 +24,7 @@ def measure_center_distance(p1, p2): DistanceInfo = collections.namedtuple('DistanceInfo', ['distance', 'point_a', 'point_b']) -def measure_distance(p1, p2): +def measure_distance(p1, p2, simulation=None): """Measures the minimum distance between two points or objects (their bodies and shapes) in meters.""" if not isinstance(p1, mpi_sim.Object) and not isinstance(p2, mpi_sim.Object): @@ -65,33 +66,56 @@ def measure_distance(p1, p2): point_b = np.array(b2_dist_info.pointB) else: - raise NotImplementedError('Calculating the distance between a point and an object is currently not supported!') - if not isinstance(p1, mpi_sim.Object) : - point_a = np.array(p1) + # To measure the distance between an object and a position we create a temporary object + # Then we measure the distance between the new temporary object and the original object + if simulation is None: + raise UserWarning('The distance measurement between an object and a position needs the simulation') + + if isinstance(p1, mpi_sim.Object) : + _object = p1 + point = p2 else : - point_a = np.array(p2) + _object = p2 + point = p1 + distance = np.inf + point_a = None point_b = None - for body_b in p2.box2d_bodies: - - for fixture_b in body_b.fixtures: + + fake_dot_obj = mpi_sim.objects.RoundTable( + position=point, + orientation=0, + radius = 0.000001, + ) + simulation.add_object(fake_dot_obj) + + for body_a in fake_dot_obj.box2d_bodies: + for fixture_a in body_a.fixtures: # fixture must have a shape - if not fixture_b.shape: + if not fixture_a.shape: continue - - try : - vertices = fixture_b.shape.vertices - distances = list(map(lambda x: np.linalg.norm(point_a-x), vertices)) - _dist = np.min(distances) - _point_b = vertices[np.argmin(distances)] - except : - _dist = np.linalg.norm(point_a-body_b.position) - fixture_b.shape.radius - _point_b = np.array(body_b.position) - - if _dist < distance: - distance = _dist - point_b = _point_b + + for body_b in _object.box2d_bodies: + for fixture_b in body_b.fixtures: + + # fixture must have a shape + if not fixture_b.shape: + continue + + b2_dist_info = Box2D.b2Distance( + shapeA=fixture_a.shape, + shapeB=fixture_b.shape, + transformA=body_a.transform, + transformB=body_b.transform + ) + + if b2_dist_info.distance < distance: + distance = b2_dist_info.distance + point_a = np.array(b2_dist_info.pointA) + point_b = np.array(b2_dist_info.pointB) + + simulation.remove_object(fake_dot_obj) return DistanceInfo(distance, point_a, point_b) -- GitLab