[Costmap] Probabilistic costmap parameters are now robot dependent.

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:

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.
     """
 

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

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()