From 40c5e235b7c7fa6622afc2d27ffc16f762b120e5 Mon Sep 17 00:00:00 2001 From: Tom Schierenbeck Date: Thu, 21 Nov 2024 09:45:01 +0100 Subject: [PATCH] [Costmap] Probabilistic costmap parameters are now robot dependent. --- src/pycram/datastructures/dataclasses.py | 12 ++++++++++++ src/pycram/pose_generator_and_validator.py | 17 +++++++++-------- src/pycram/probabilistic_costmap.py | 13 ++++++++----- test/test_costmaps.py | 2 +- 4 files changed, 30 insertions(+), 14 deletions(-) diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index 0e91d3fcf..395260ad2 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -145,6 +145,18 @@ def get_max(self) -> List[float]: """ return [self.max_x, self.max_y, self.max_z] + @property + def width(self) -> float: + return self.max_x - self.min_x + + @property + def height(self) -> float: + return self.max_z - self.min_z + + @property + def depth(self) -> float: + return self.max_y - self.min_y + @dataclass class CollisionCallbacks: diff --git a/src/pycram/pose_generator_and_validator.py b/src/pycram/pose_generator_and_validator.py index 6672b6c6c..1232ee5b0 100644 --- a/src/pycram/pose_generator_and_validator.py +++ b/src/pycram/pose_generator_and_validator.py @@ -1,22 +1,23 @@ import numpy as np import tf +from typing_extensions import Tuple, List, Union, Dict, Iterable -from .datastructures.world import World -from .world_concepts.world_object import Object -from .world_reasoning import contact from .costmaps import Costmap -from .local_transformer import LocalTransformer from .datastructures.pose import Pose, Transform -from .robot_description import RobotDescription +from .datastructures.world import World from .external_interfaces.ik import request_ik from .failures import IKError -from typing_extensions import Tuple, List, Union, Dict, Iterable +from .local_transformer import LocalTransformer +from .robot_description import RobotDescription +from .world_concepts.world_object import Object +from .world_reasoning import contact class PoseGenerator: """ - Crates pose candidates from a given costmap. The generator - selects the highest values, amount is given by number_of_sample, and returns the corresponding positions. + Creates pose candidates from a given costmap. + The generator selects the highest values, amount is given by number_of_sample, and returns the corresponding + positions. Orientations are calculated such that the Robot faces the center of the costmap. """ diff --git a/src/pycram/probabilistic_costmap.py b/src/pycram/probabilistic_costmap.py index c2597be04..dae6a9baa 100644 --- a/src/pycram/probabilistic_costmap.py +++ b/src/pycram/probabilistic_costmap.py @@ -10,13 +10,14 @@ from typing_extensions import Optional, Type from visualization_msgs.msg import Marker, MarkerArray -from . import World +from .datastructures.world import World from .costmaps import Costmap, OccupancyCostmap, VisibilityCostmap import matplotlib.colorbar from .datastructures.pose import Pose from .ros.data_types import Duration from .ros.publisher import create_publisher from .units import meter, centimeter +from .robot_description import RobotDescription from pint import Unit, Quantity from probabilistic_model.probabilistic_circuit.nx.helper import uniform_measure_of_event, fully_factorized @@ -80,20 +81,22 @@ def __init__(self, origin: Pose, resolution = self.size.to(meter) / number_of_cells if costmap_type == OccupancyCostmap: + robot_bounding_box = self.world.robot.get_axis_aligned_bounding_box() + distance_to_obstacle = max(robot_bounding_box.width, robot_bounding_box.depth) / 2 self.costmap = OccupancyCostmap( origin=self.origin, - distance_to_obstacle=0.2, + distance_to_obstacle=distance_to_obstacle, size=number_of_cells, resolution=resolution.magnitude, from_ros=False, world = self.world) elif costmap_type == VisibilityCostmap: + camera = list(self.world.robot_description.cameras.values())[0] self.costmap = VisibilityCostmap( - min_height=0, max_height=2, size=number_of_cells, resolution=resolution.magnitude, - origin=self.origin, world=self.world) + min_height=camera.minimal_height, max_height=camera.maximal_height, size=number_of_cells, + resolution=resolution.magnitude, origin=self.origin, world=self.world) else: raise NotImplementedError(f"Unknown costmap type {costmap_type}") - self.create_distribution() @cached_property diff --git a/test/test_costmaps.py b/test/test_costmaps.py index 5c36052a2..d589a9a90 100644 --- a/test/test_costmaps.py +++ b/test/test_costmaps.py @@ -92,7 +92,7 @@ def test_iterate(self): class ProbabilisticCostmapTestCase(BulletWorldTestCase): - origin = Pose([0, 1, 0], [0, 0, 0, 1]) + origin = Pose([1.5, 1, 0], [0, 0, 0, 1]) def setUp(self): super().setUp()