added ball radius (not working)

bit-bots · Nov 20, 2024 · 7197465 · 7197465
1 parent c65f734
commit 7197465
Showing 1 changed file with 11 additions and 2 deletions.
diff --git a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/map.py b/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/map.py
@@ -39,6 +39,7 @@ def __init__(self, node: Node, buffer: tf2.Buffer) -> None:
         self.config_obstacle_value: int = self.node.config.map.obstacle_value
         self.ball_obstacle_active: bool = True
         self._obstacles: list[Geometry]
+        self._ball_geometry = list[Geometry]
         self._obstacles_union: Geometry | None = None
 
     def obstacles(self) -> list[Geometry]:
@@ -64,6 +65,7 @@ def set_ball(self, ball: PoseWithCovarianceStamped) -> None:
             self.ball_buffer = [self.buffer.transform(point, self.frame).point]
         except (tf2.ConnectivityException, tf2.LookupException, tf2.ExtrapolationException) as e:
             self.node.get_logger().warn(str(e))
+        self._update_geometries()
 
     def _render_balls(self, map: np.ndarray) -> None:
         """
@@ -95,16 +97,23 @@ def set_robots(self, robots: sv3dm.RobotArray):
             except (tf2.ConnectivityException, tf2.LookupException, tf2.ExtrapolationException) as e:
                 self.node.get_logger().warn(str(e))
         self.robot_buffer = new_buffer
-        self._update_robot_geometries()
+        self._update_geometries()
 
-    def _update_robot_geometries(self):
+    def _update_geometries(self):
         self._obstacles = []
         for robot in self.robot_buffer:
             center = shapely.Point(robot.bb.center.position.x, robot.bb.center.position.y)
             radius = max(numpify(robot.bb.size)[:2]) / 2
             dilation = self.config_inflation_dialation / self.node.config.map.resolution
             geometry = center.buffer(radius + dilation, quad_segs=CIRCLE_APPROXIMATION_SEGMENTS // 4)
             self._obstacles.append(geometry)
+        if self.ball_obstacle_active:
+            for ball in self.ball_buffer:
+                center = shapely.Point(ball.x, ball.y)
+                radius = self.config_ball_diameter / 2
+                dilation = self.config_inflation_dialation / self.node.config.map.resolution
+                geometry = center.buffer(radius + dilation, quad_segs=CIRCLE_APPROXIMATION_SEGMENTS // 4)
+                self._obstacles.append(geometry)
         self._obstacles_union = shapely.union_all(self._obstacles)
 
     def _render_robots(self, map: np.ndarray) -> None: