Removed costmap from map state

bitbots_navigation/bitbots_path_planning/bitbots_path_planning/map.py

@@ -18,18 +18,13 @@ class Map:
     def __init__(self, node: Node, buffer: tf2.Buffer) -> None:
         self.node = node
         self.buffer = buffer
-        self.resolution: int = self.node.config.map.resolution
         parameters = get_parameters_from_other_node(
             self.node, "/parameter_blackboard", ["field.size.x", "field.size.y", "field.size.padding"]
         )
         self.size: tuple[float, float] = (
             parameters["field.size.x"] + 2 * parameters["field.size.padding"],
             parameters["field.size.y"] + 2 * parameters["field.size.padding"],
         )
-        self.map: np.ndarray = np.ones(
-            (np.array(self.size) * self.resolution).astype(int),
-            dtype=np.int8,
-        )
 
         self.frame: str = self.node.config.map.planning_frame
         self.ball_buffer: list[Point] = []
@@ -52,16 +47,16 @@ def set_ball(self, ball: PoseWithCovarianceStamped) -> None:
         except (tf2.ConnectivityException, tf2.LookupException, tf2.ExtrapolationException) as e:
             self.node.get_logger().warn(str(e))
 
-    def _render_balls(self) -> None:
+    def _render_balls(self, map: np.ndarray) -> None:
         """
         Draws the ball buffer onto the costmap
         """
         ball: sv3dm.Ball
         for ball in self.ball_buffer:
             cv2.circle(
-                self.map,
+                map,
                 self.to_map_space(ball.x, ball.y)[::-1],
-                round(self.config_ball_diameter / 2 * self.resolution),
+                round(self.config_ball_diameter / 2 * self.node.config.map.resolution),
                 self.config_obstacle_value,
                 -1,
             )
@@ -83,16 +78,16 @@ def set_robots(self, robots: sv3dm.RobotArray):
                 self.node.get_logger().warn(str(e))
         self.robot_buffer = new_buffer
 
-    def _render_robots(self) -> None:
+    def _render_robots(self, map: np.ndarray) -> None:
         """
         Draws the robot buffer onto the costmap
         """
         robot: sv3dm.Robot
         for robot in self.robot_buffer:
             cv2.circle(
-                self.map,
+                map,
                 self.to_map_space(robot.bb.center.position.x, robot.bb.center.position.y)[::-1],
-                round(max(numpify(robot.bb.size)[:2]) * self.resolution),
+                round(max(numpify(robot.bb.size)[:2]) * self.node.config.map.resolution),
                 self.config_obstacle_value,
                 -1,
             )
@@ -102,62 +97,69 @@ def to_map_space(self, x: float, y: float) -> tuple[int, int]:
         Maps a point (x, y in meters) to corresponding pixel on the costmap
         """
         return (
-            max(0, min(round((x - self.get_origin()[0]) * self.resolution), self.map.shape[0] - 1)),
-            max(0, min(round((y - self.get_origin()[1]) * self.resolution), self.map.shape[1] - 1)),
+            max(
+                0,
+                min(
+                    round((x - self.get_origin()[0]) * self.node.config.map.resolution),
+                    self.size[0] * self.node.config.map.resolution - 1,
+                ),
+            ),
+            max(
+                0,
+                min(
+                    round((y - self.get_origin()[1]) * self.node.config.map.resolution),
+                    self.size[1] * self.node.config.map.resolution - 1,
+                ),
+            ),
         )
 
     def from_map_space_np(self, points: np.ndarray) -> np.ndarray:
         """
         Maps an array of pixel coordinates from the costmap to world coordinates (meters)
         """
-        return points / self.resolution + self.get_origin()
+        return points / self.node.config.map.resolution + self.get_origin()
 
     def get_origin(self) -> np.ndarray:
         """
         Origin of the costmap in meters
         """
-        return np.array([-self.map.shape[0] / self.resolution / 2, -self.map.shape[1] / self.resolution / 2])
+        return np.array(
+            [
+                -(self.size[0] * self.node.config.map.resolution) / self.node.config.map.resolution / 2,
+                -(self.size[1] * self.node.config.map.resolution) / self.node.config.map.resolution / 2,
+            ]
+        )
 
-    def clear(self) -> None:
+    @property
+    def costmap(self) -> np.ndarray:
         """
-        Clears the complete cost map
+        Generate the complete cost map
         """
-        self.map[...] = 1
+        map: np.ndarray = np.ones((np.array(self.size) * self.node.config.map.resolution).astype(int), dtype=np.int8)
+        if self.ball_obstacle_active:
+            self._render_balls(map)
+        self._render_robots(map)
+        return self.inflate(map)
 
-    def inflate(self) -> None:
+    def inflate(self, map: np.ndarray) -> np.ndarray:
         """
         Applies inflation to all occupied areas of the costmap
         """
-        idx = self.map == 1
-        map = cv2.dilate(
-            self.map.astype(np.uint8),
+        idx = map == 1
+        nmap = cv2.dilate(
+            map.astype(np.uint8),
             np.ones((self.config_inflation_dialation, self.config_inflation_dialation), np.uint8),
             iterations=2,
         )
-        self.map[idx] = cv2.blur(map, (self.config_inflation_blur, self.config_inflation_blur)).astype(np.int8)[idx]
-
-    def update(self) -> None:
-        """
-        Regenerates the costmap based on the ball and robot buffer
-        """
-        self.clear()
-        if self.ball_obstacle_active:
-            self._render_balls()
-        self._render_robots()
-        self.inflate()
+        map[idx] = cv2.blur(nmap, (self.config_inflation_blur, self.config_inflation_blur)).astype(np.int8)[idx]
+        return map
 
     def avoid_ball(self, state: bool) -> None:
         """
         Activates or deactivates the ball obstacle
         """
         self.ball_obstacle_active = state
 
-    def get_map(self) -> np.ndarray:
-        """
-        Returns the costmap as an numpy array
-        """
-        return self.map
-
     def get_frame(self) -> str:
         """
         Returns the frame of reference of the map
@@ -168,11 +170,12 @@ def to_msg(self) -> OccupancyGrid:
         """
         Returns the costmap as an OccupancyGrid message
         """
-        msg: OccupancyGrid = msgify(OccupancyGrid, self.get_map().T)
+        map = self.costmap
+        msg: OccupancyGrid = msgify(OccupancyGrid, map)
         msg.header.frame_id = self.frame
-        msg.info.width = self.map.shape[0]
-        msg.info.height = self.map.shape[1]
-        msg.info.resolution = 1 / self.resolution
+        msg.info.width = map.shape[0]
+        msg.info.height = map.shape[1]
+        msg.info.resolution = 1 / self.node.config.map.resolution
         msg.info.origin.position.x = self.get_origin()[0]
         msg.info.origin.position.y = self.get_origin()[1]
         return msg

bitbots_navigation/bitbots_path_planning/bitbots_path_planning/path_planning.py

@@ -88,8 +88,6 @@ def step(self) -> None:
             self.param_listener.refresh_dynamic_parameters()
             self.config = self.param_listener.get_params()
         try:
-            # Update the map with the latest ball and robot positions
-            self.map.update()
             # Publish the costmap for visualization
             self.costmap_pub.publish(self.map.to_msg())
 
@@ -106,6 +104,7 @@ def step(self) -> None:
                 self.carrot_pub.publish(carrot_point)
         except Exception as e:
             self.get_logger().error(f"Caught exception during calculation of path planning step: {e}")
+            raise e
 
 
 def main(args=None):

bitbots_navigation/bitbots_path_planning/bitbots_path_planning/planner.py

@@ -51,7 +51,7 @@ def step(self) -> Path:
         goal = self.goal
 
         # Get current costmap
-        navigation_grid = self.map.get_map()
+        navigation_grid = self.map.costmap
 
         # Get my pose and position on the map
         my_position = self.buffer.lookup_transform(