diff --git a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/map.py b/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/map.py index 44986bb6b..57e0d5ec9 100644 --- a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/map.py +++ b/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/map.py @@ -18,7 +18,6 @@ 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"] ) @@ -26,10 +25,6 @@ def __init__(self, node: Node, buffer: tf2.Buffer) -> None: 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,49 +97,62 @@ 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: """ @@ -152,12 +160,6 @@ def avoid_ball(self, state: bool) -> None: """ 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 diff --git a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/path_planning.py b/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/path_planning.py index e655ae375..d5e9b0d82 100644 --- a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/path_planning.py +++ b/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): diff --git a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/planner.py b/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/planner.py index 9c3254d0e..a0ee11696 100644 --- a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/planner.py +++ b/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(