Added A* on visibility graph

bitbots_navigation/bitbots_path_planning/bitbots_path_planning/map.py

@@ -1,14 +1,18 @@
 import cv2
 import numpy as np
+import shapely
 import soccer_vision_3d_msgs.msg as sv3dm
 import tf2_ros as tf2
 from bitbots_utils.utils import get_parameters_from_other_node
 from geometry_msgs.msg import Point
 from nav_msgs.msg import OccupancyGrid
 from rclpy.node import Node
 from ros2_numpy import msgify, numpify
+from shapely import Geometry
 from tf2_geometry_msgs import PointStamped, PoseWithCovarianceStamped
 
+CIRCLE_APPROXIMATION_SEGMENTS = 12
+
 
 class Map:
     """
@@ -34,6 +38,20 @@ def __init__(self, node: Node, buffer: tf2.Buffer) -> None:
         self.config_inflation_dialation: int = self.node.config.map.inflation.dialate
         self.config_obstacle_value: int = self.node.config.map.obstacle_value
         self.ball_obstacle_active: bool = True
+        self._obstacles: list[Geometry]
+        self._obstacles_union: Geometry | None = None
+
+    def obstacles(self) -> list[Geometry]:
+        """
+        Return the obstacles in the map as Geometry objects
+        """
+        return self._obstacles
+
+    def intersects(self, object: Geometry) -> bool:
+        """
+        Check if an object intersects with any obstacles in the map
+        """
+        return not self._obstacles_union.touches(object) and self._obstacles_union.intersects(object)
 
     def set_ball(self, ball: PoseWithCovarianceStamped) -> None:
         """
@@ -77,6 +95,17 @@ 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()
+
+    def _update_robot_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)
+        self._obstacles_union = shapely.union_all(self._obstacles)
 
     def _render_robots(self, map: np.ndarray) -> None:
         """

bitbots_navigation/bitbots_path_planning/bitbots_path_planning/path_planning.py

@@ -12,7 +12,7 @@
 from bitbots_path_planning.controller import Controller
 from bitbots_path_planning.map import Map
 from bitbots_path_planning.path_planning_parameters import bitbots_path_planning as parameters
-from bitbots_path_planning.planner import Planner
+from bitbots_path_planning.planner import VisibilityPlanner
 
 
 class PathPlanning(Node):
@@ -30,7 +30,7 @@ def __init__(self) -> None:
 
         # Create submodules
         self.map = Map(node=self, buffer=self.tf_buffer)
-        self.planner = Planner(node=self, buffer=self.tf_buffer, map=self.map)
+        self.planner = VisibilityPlanner(node=self, buffer=self.tf_buffer, map=self.map)
         self.controller = Controller(node=self, buffer=self.tf_buffer)
 
         # Subscriber
@@ -54,7 +54,7 @@ def __init__(self) -> None:
         self.create_subscription(
             Empty,
             "pathfinding/cancel",
-            lambda _: self.planner.cancel(),
+            lambda _: self.planner.cancel_goal(),
             5,
             callback_group=MutuallyExclusiveCallbackGroup(),
         )
@@ -104,7 +104,6 @@ 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

@@ -1,17 +1,133 @@
+from abc import ABC, abstractmethod
+
 import numpy as np
 import pyastar2d
 import tf2_ros as tf2
 from geometry_msgs.msg import PoseStamped
+from heapdict import heapdict
 from nav_msgs.msg import Path
 from rclpy.duration import Duration
 from rclpy.node import Node
 from rclpy.time import Time
+from shapely import LineString, Point, distance
 from std_msgs.msg import Header
 
 from bitbots_path_planning.map import Map
 
 
-class Planner:
+class Planner(ABC):
+    @abstractmethod
+    def set_goal(self, pose: PoseStamped) -> None:
+        pass
+
+    @abstractmethod
+    def cancel_goal(self) -> None:
+        pass
+
+    @abstractmethod
+    def active(self) -> bool:
+        pass
+
+    @abstractmethod
+    def step(self) -> Path:
+        pass
+
+
+class VisibilityNode:
+    """
+    A Node that stores all the information needed for A-Star
+    """
+
+    def __init__(self, point: Point, cost=float("inf"), predecessor=None):
+        """
+        Initializes a Node at given Point
+        """
+        self.point = point
+        self.cost = cost
+        self.predecessor = predecessor
+
+
+class VisibilityPlanner(Planner):
+    def __init__(self, node: Node, buffer: tf2.Buffer, map: Map) -> None:
+        self.node = node
+        self.buffer = buffer
+        self.map = map
+        self.goal: PoseStamped | None = None
+        self.base_footprint_frame: str = self.node.config.base_footprint_frame
+
+    def set_goal(self, pose: PoseStamped) -> None:
+        """
+        Updates the goal pose
+        """
+        pose.header.stamp = Time(clock_type=self.node.get_clock().clock_type).to_msg()
+        self.goal = pose
+
+    def cancel_goal(self) -> None:
+        """
+        Removes the current goal
+        """
+        self.goal = None
+        self.path = None
+
+    def active(self) -> bool:
+        """
+        Determine if we have an active goal
+        """
+        return self.goal is not None
+
+    def step(self) -> Path:
+        goal = Point(self.goal.pose.position.x, self.goal.pose.position.y)
+        my_position = self.buffer.lookup_transform(
+            self.map.frame, self.base_footprint_frame, Time(), Duration(seconds=0.2)
+        ).transform.translation
+        start = Point(my_position.x, my_position.y)
+
+        open_list = heapdict()
+        possible_successor_nodes = [VisibilityNode(start, 0.0), VisibilityNode(goal)]
+
+        for obstacle in self.map.obstacles():
+            for x, y in set(obstacle.boundary.coords):
+                possible_successor_nodes.append(VisibilityNode(Point(x, y)))
+
+        open_list[possible_successor_nodes[0]] = distance(start, goal)
+
+        while len(open_list) != 0:
+            current_node = open_list.popitem()
+            if current_node[0].point == goal:
+                path = [goal]
+                next_node = current_node[0].predecessor
+                while not next_node.point == start:
+                    path.append(next_node.point)
+                    next_node = next_node.predecessor
+                path.append(start)
+                poses = []
+                for pos in reversed(path):
+                    pose = PoseStamped()
+                    pose.pose.position.x = pos.x
+                    pose.pose.position.y = pos.y
+                    poses.append(pose)
+                return Path(
+                    header=Header(frame_id=self.map.get_frame(), stamp=self.node.get_clock().now().to_msg()),
+                    poses=poses,
+                )  # create and return final LineString
+            possible_successor_nodes.remove(current_node[0])
+
+            # expand current node
+            successors = []
+            for node in possible_successor_nodes:  # find all correct successors of current node
+                if not (self.map.intersects(LineString([current_node[0].point, node.point]))):
+                    successors.append(node)
+            for successor in successors:
+                tentative_g = current_node[0].cost + distance(current_node[0].point, successor.point)
+                if successor in open_list and tentative_g >= successor.cost:
+                    continue
+                successor.predecessor = current_node[0]
+                successor.cost = tentative_g
+                f = tentative_g + distance(node.point, goal)  # calculate new f-value (costs + heuristic)
+                open_list[successor] = f
+
+
+class GridPlanner(Planner):
     """
     A simple grid based A* interface
     """
@@ -21,7 +137,6 @@ def __init__(self, node: Node, buffer: tf2.Buffer, map: Map) -> None:
         self.buffer = buffer
         self.map = map
         self.goal: PoseStamped | None = None
-        self.path: Path | None = None
         self.base_footprint_frame: str = self.node.config.base_footprint_frame
 
     def set_goal(self, pose: PoseStamped) -> None:
@@ -31,7 +146,7 @@ def set_goal(self, pose: PoseStamped) -> None:
         pose.header.stamp = Time(clock_type=self.node.get_clock().clock_type).to_msg()
         self.goal = pose
 
-    def cancel(self) -> None:
+    def cancel_goal(self) -> None:
         """
         Removes the current goal
         """
@@ -83,14 +198,6 @@ def to_pose_msg(element):
         poses = list(map(to_pose_msg, path))
 
         poses.append(goal)
-        self.path = Path(
+        return Path(
             header=Header(frame_id=self.map.get_frame(), stamp=self.node.get_clock().now().to_msg()), poses=poses
         )
-
-        return self.path
-
-    def get_path(self) -> Path | None:
-        """
-        Returns the most recent path
-        """
-        return self.path

requirements/common.txt

@@ -3,6 +3,7 @@ pip
 transforms3d==0.4.1
 git+https://github.com/Flova/pyastar2d
 git+https://github.com/bit-bots/YOEO
+heapdict==1.0.1
 simpleeval
 
 # The following dependencies are only needed for rl walking and we don't actively use them currently