From 594a71e99309edc4a69f1b3060780c7cd7b35ac6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Helena=20J=C3=A4ger?= Date: Wed, 13 Nov 2024 14:52:13 +0100 Subject: [PATCH 01/47] feature/projekt24/pathfinding erstellt From b5e4a83fc5d99d25fcc665ab95eebb59cd9d7351 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Helena=20J=C3=A4ger?= Date: Wed, 13 Nov 2024 16:21:43 +0100 Subject: [PATCH 02/47] Removed costmap from map state --- .../bitbots_path_planning/map.py | 89 ++++++++++--------- .../bitbots_path_planning/path_planning.py | 3 +- .../bitbots_path_planning/planner.py | 2 +- 3 files changed, 48 insertions(+), 46 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 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( From b27052fbdd39f882f97b75cb5bfce194b6ad7040 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Helena=20J=C3=A4ger?= Date: Wed, 13 Nov 2024 17:57:04 +0100 Subject: [PATCH 03/47] Added A* on visibility graph --- .../bitbots_path_planning/map.py | 29 ++++ .../bitbots_path_planning/path_planning.py | 7 +- .../bitbots_path_planning/planner.py | 131 ++++++++++++++++-- requirements/common.txt | 1 + 4 files changed, 152 insertions(+), 16 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 index 57e0d5ec9..fbeea88df 100644 --- a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/map.py +++ b/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/map.py @@ -1,5 +1,6 @@ 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 @@ -7,8 +8,11 @@ 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: """ 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 d5e9b0d82..5c69297db 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 @@ -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): 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 a0ee11696..cae66a4f6 100644 --- a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/planner.py +++ b/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 diff --git a/requirements/common.txt b/requirements/common.txt index 79e894b3e..6f1f57acf 100644 --- a/requirements/common.txt +++ b/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 From b7f7a453e419f276f13aa4a1f227286dfd5fadde Mon Sep 17 00:00:00 2001 From: Lea Date: Wed, 20 Nov 2024 12:32:59 +0100 Subject: [PATCH 04/47] fixed calculation of dilation in _update_robot_geometries --- .../bitbots_path_planning/bitbots_path_planning/map.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 fbeea88df..1d18adc2e 100644 --- a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/map.py +++ b/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/map.py @@ -102,7 +102,7 @@ def _update_robot_geometries(self): 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 + 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) From 8afb027463beec5cd65544bca5a3f27e152ae7fe Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Helena=20J=C3=A4ger?= Date: Wed, 20 Nov 2024 14:10:23 +0100 Subject: [PATCH 05/47] added rviz2 test file for path planning --- .../config/path_planning_viz.rviz | 486 ++++++++++++++++++ 1 file changed, 486 insertions(+) create mode 100644 bitbots_navigation/bitbots_path_planning/config/path_planning_viz.rviz diff --git a/bitbots_navigation/bitbots_path_planning/config/path_planning_viz.rviz b/bitbots_navigation/bitbots_path_planning/config/path_planning_viz.rviz new file mode 100644 index 000000000..0a5fc51ed --- /dev/null +++ b/bitbots_navigation/bitbots_path_planning/config/path_planning_viz.rviz @@ -0,0 +1,486 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /Grid1 + - /Map1 + - /Map1/Topic1 + - /Path1 + - /Pose1 + Splitter Ratio: 0.5 + Tree Height: 1586 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 20 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + camera: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_optical_frame_uncalibrated: + Alpha: 1 + Show Axes: false + Show Trail: false + head: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + imu_frame_2: + Alpha: 1 + Show Axes: false + Show Trail: false + imu_frame_uncalibrated: + Alpha: 1 + Show Axes: false + Show Trail: false + l_ankle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_cleat_l_back: + Alpha: 1 + Show Axes: false + Show Trail: false + l_cleat_l_front: + Alpha: 1 + Show Axes: false + Show Trail: false + l_cleat_r_back: + Alpha: 1 + Show Axes: false + Show Trail: false + l_cleat_r_front: + Alpha: 1 + Show Axes: false + Show Trail: false + l_foot: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_hip_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_hip_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_lower_arm: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_lower_leg: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_shoulder: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_sole: + Alpha: 1 + Show Axes: false + Show Trail: false + l_toe: + Alpha: 1 + Show Axes: false + Show Trail: false + l_upper_arm: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_upper_leg: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist: + Alpha: 1 + Show Axes: false + Show Trail: false + llb: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + llf: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lrb: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lrf: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + neck: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_ankle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_cleat_l_back: + Alpha: 1 + Show Axes: false + Show Trail: false + r_cleat_l_front: + Alpha: 1 + Show Axes: false + Show Trail: false + r_cleat_r_back: + Alpha: 1 + Show Axes: false + Show Trail: false + r_cleat_r_front: + Alpha: 1 + Show Axes: false + Show Trail: false + r_foot: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_hip_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_hip_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_lower_arm: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_lower_leg: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_shoulder: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_sole: + Alpha: 1 + Show Axes: false + Show Trail: false + r_toe: + Alpha: 1 + Show Axes: false + Show Trail: false + r_upper_arm: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_upper_leg: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist: + Alpha: 1 + Show Axes: false + Show Trail: false + rlb: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rlf: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rrb: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rrf: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + torso: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Alpha: 0.699999988079071 + Binary representation: true + Binary threshold: 100 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: + Depth: 5 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /field/map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /field/map_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 129; 61; 156 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /path + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz_default_plugins/Pose + Color: 51; 209; 122 + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: Pose + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz_default_plugins/PoseWithCovariance + Color: 255; 25; 0 + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: PoseWithCovariance + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /pose_with_covariance + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Angle: 1.579999566078186 + Class: rviz_default_plugins/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Scale: 108.03849029541016 + Target Frame: + Value: TopDownOrtho (rviz_default_plugins) + X: 0.3327471911907196 + Y: 0.0025227859150618315 + Saved: ~ +Window Geometry: + Displays: + collapsed: true + Height: 1883 + Hide Left Dock: true + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001b0000006bdfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d000006bd000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000006bdfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000006bd000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002fb00fffffffb0000000800540069006d006501000000000000045000000000000000000000039b000006bd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1200 + X: 1920 + Y: 0 From c65f7346acd80895405ae1b060a7976485f945da Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Helena=20J=C3=A4ger?= Date: Wed, 20 Nov 2024 14:29:06 +0100 Subject: [PATCH 06/47] added goal pose to visibility graph path --- .../bitbots_path_planning/bitbots_path_planning/planner.py | 1 + 1 file changed, 1 insertion(+) 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 cae66a4f6..413971488 100644 --- a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/planner.py +++ b/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/planner.py @@ -106,6 +106,7 @@ def step(self) -> Path: pose.pose.position.x = pos.x pose.pose.position.y = pos.y poses.append(pose) + poses.append(self.goal) return Path( header=Header(frame_id=self.map.get_frame(), stamp=self.node.get_clock().now().to_msg()), poses=poses, From 7197465996e636cf86192e049efda2b304fc91a6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Helena=20J=C3=A4ger?= Date: Wed, 20 Nov 2024 14:49:36 +0100 Subject: [PATCH 07/47] added ball radius (not working) --- .../bitbots_path_planning/map.py | 13 +++++++++++-- 1 file changed, 11 insertions(+), 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 index 1d18adc2e..34bc6fcd6 100644 --- 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,9 +97,9 @@ 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) @@ -105,6 +107,13 @@ def _update_robot_geometries(self): 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: From 3496a93831d84663079d7576174f589caf9b2a1a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Helena=20J=C3=A4ger?= Date: Wed, 20 Nov 2024 14:54:55 +0100 Subject: [PATCH 08/47] removed A* grid implementation --- .../bitbots_path_planning/map.py | 109 +----------------- .../bitbots_path_planning/path_planning.py | 6 +- .../bitbots_path_planning/planner.py | 78 ------------- 3 files changed, 2 insertions(+), 191 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 index 34bc6fcd6..ce28b4473 100644 --- a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/map.py +++ b/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/map.py @@ -1,13 +1,10 @@ -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 ros2_numpy import numpify from shapely import Geometry from tf2_geometry_msgs import PointStamped, PoseWithCovarianceStamped @@ -67,20 +64,6 @@ def set_ball(self, ball: PoseWithCovarianceStamped) -> None: self.node.get_logger().warn(str(e)) self._update_geometries() - 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( - map, - self.to_map_space(ball.x, ball.y)[::-1], - round(self.config_ball_diameter / 2 * self.node.config.map.resolution), - self.config_obstacle_value, - -1, - ) - def set_robots(self, robots: sv3dm.RobotArray): """ Adds a given robot array to the robot buffer @@ -116,82 +99,6 @@ def _update_geometries(self): self._obstacles.append(geometry) self._obstacles_union = shapely.union_all(self._obstacles) - 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( - map, - self.to_map_space(robot.bb.center.position.x, robot.bb.center.position.y)[::-1], - round(max(numpify(robot.bb.size)[:2]) * self.node.config.map.resolution), - self.config_obstacle_value, - -1, - ) - - 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.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.node.config.map.resolution + self.get_origin() - - def get_origin(self) -> np.ndarray: - """ - Origin of the costmap in meters - """ - 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, - ] - ) - - @property - def costmap(self) -> np.ndarray: - """ - Generate the complete cost map - """ - 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, map: np.ndarray) -> np.ndarray: - """ - Applies inflation to all occupied areas of the costmap - """ - idx = map == 1 - nmap = cv2.dilate( - map.astype(np.uint8), - np.ones((self.config_inflation_dialation, self.config_inflation_dialation), np.uint8), - iterations=2, - ) - 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 @@ -203,17 +110,3 @@ def get_frame(self) -> str: Returns the frame of reference of the map """ return self.frame - - def to_msg(self) -> OccupancyGrid: - """ - Returns the costmap as an OccupancyGrid message - """ - map = self.costmap - msg: OccupancyGrid = msgify(OccupancyGrid, map) - msg.header.frame_id = self.frame - 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 5c69297db..1afefea9f 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 @@ -2,7 +2,7 @@ import soccer_vision_3d_msgs.msg as sv3dm from bitbots_tf_buffer import Buffer from geometry_msgs.msg import PointStamped, PoseStamped, PoseWithCovarianceStamped, Twist -from nav_msgs.msg import OccupancyGrid, Path +from nav_msgs.msg import Path from rclpy.callback_groups import MutuallyExclusiveCallbackGroup from rclpy.duration import Duration from rclpy.executors import MultiThreadedExecutor @@ -68,7 +68,6 @@ def __init__(self) -> None: # Publisher self.cmd_vel_pub = self.create_publisher(Twist, "cmd_vel", 1) - self.costmap_pub = self.create_publisher(OccupancyGrid, "costmap", 1) self.path_pub = self.create_publisher(Path, "path", 1) self.carrot_pub = self.create_publisher(PointStamped, "carrot", 1) @@ -88,9 +87,6 @@ def step(self) -> None: self.param_listener.refresh_dynamic_parameters() self.config = self.param_listener.get_params() try: - # Publish the costmap for visualization - self.costmap_pub.publish(self.map.to_msg()) - if self.planner.active(): # Calculate the path to the goal pose considering the current map path = self.planner.step() 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 413971488..37efe3750 100644 --- a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/planner.py +++ b/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/planner.py @@ -1,7 +1,5 @@ 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 @@ -126,79 +124,3 @@ def step(self) -> Path: 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 - """ - - 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: - """ - Generates a new A* path to the goal pose with respect to the costmap - """ - goal = self.goal - - # Get current costmap - navigation_grid = self.map.costmap - - # Get my pose and position on the map - my_position = self.buffer.lookup_transform( - self.map.frame, self.base_footprint_frame, Time(), Duration(seconds=0.2) - ).transform.translation - - # Transform goal pose to map frame if needed - if goal.header.frame_id != self.map.frame: - goal = self.buffer.transform(goal, self.map.frame, timeout=Duration(seconds=0.2)) - - # Run A* from our current position to the goal position - path = pyastar2d.astar_path( - navigation_grid.astype(np.float32), - self.map.to_map_space(my_position.x, my_position.y), - self.map.to_map_space(goal.pose.position.x, goal.pose.position.y), - allow_diagonal=False, - ) - - # Convert the pixel coordinates to world coordinates - path = self.map.from_map_space_np(path) - - # Build path message - def to_pose_msg(element): - pose = PoseStamped() - pose.pose.position.x = element[0] - pose.pose.position.y = element[1] - return pose - - poses = list(map(to_pose_msg, path)) - - poses.append(goal) - return Path( - header=Header(frame_id=self.map.get_frame(), stamp=self.node.get_clock().now().to_msg()), poses=poses - ) From 4ecf8030b63effc6e974332ab22b9bbc165c130a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Helena=20J=C3=A4ger?= Date: Wed, 20 Nov 2024 15:08:53 +0100 Subject: [PATCH 09/47] Map class cleanup --- .../bitbots_path_planning/map.py | 45 ++++++++++--------- 1 file changed, 25 insertions(+), 20 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 index ce28b4473..14235473c 100644 --- a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/map.py +++ b/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/map.py @@ -2,7 +2,6 @@ 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 rclpy.node import Node from ros2_numpy import numpify from shapely import Geometry @@ -13,7 +12,7 @@ class Map: """ - Costmap that keeps track of obstacles like the ball or other robots. + Obstacle Map that keeps track of obstacles like the ball or other robots. """ def __init__(self, node: Node, buffer: tf2.Buffer) -> None: @@ -28,28 +27,26 @@ def __init__(self, node: Node, buffer: tf2.Buffer) -> None: ) self.frame: str = self.node.config.map.planning_frame - self.ball_buffer: list[Point] = [] - self.robot_buffer: list[sv3dm.Robot] = [] self.config_ball_diameter: float = self.node.config.map.ball_diameter self.config_inflation_blur: int = self.node.config.map.inflation.blur 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._ball_geometry = list[Geometry] - self._obstacles_union: Geometry | None = None + self._robot_geometries: list[Geometry] = [] + self._ball_geometries: list[Geometry] = [] + self._obstacle_union: Geometry | None = None def obstacles(self) -> list[Geometry]: """ Return the obstacles in the map as Geometry objects """ - return self._obstacles + return self._robot_geometries + self._ball_geometries 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) + return not self._obstacle_union.touches(object) and self._obstacle_union.intersects(object) def set_ball(self, ball: PoseWithCovarianceStamped) -> None: """ @@ -58,11 +55,13 @@ def set_ball(self, ball: PoseWithCovarianceStamped) -> None: point = PointStamped() point.header.frame_id = ball.header.frame_id point.point = ball.pose.pose.position + ball_buffer = [] try: - self.ball_buffer = [self.buffer.transform(point, self.frame).point] + 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() + self._update_ball_geometries(ball_buffer) + self._update_obstacle_union() def set_robots(self, robots: sv3dm.RobotArray): """ @@ -79,25 +78,31 @@ def set_robots(self, robots: sv3dm.RobotArray): new_buffer.append(robot) except (tf2.ConnectivityException, tf2.LookupException, tf2.ExtrapolationException) as e: self.node.get_logger().warn(str(e)) - self.robot_buffer = new_buffer - self._update_geometries() + self._update_robot_geometries(new_buffer) + self._update_obstacle_union() - def _update_geometries(self): - self._obstacles = [] - for robot in self.robot_buffer: + def _update_robot_geometries(self, robots): + self._robot_geometries = [] + for robot in robots: + print(robot) 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._robot_geometries.append(geometry) + + def _update_ball_geometries(self, balls): + self._ball_geometries = [] if self.ball_obstacle_active: - for ball in self.ball_buffer: + for ball in balls: 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) + self._ball_geometries.append(geometry) + + def _update_obstacle_union(self): + self._obstacle_union = shapely.union_all(self._ball_geometries + self._robot_geometries) def avoid_ball(self, state: bool) -> None: """ From 0b86779227569c417908d1344b8751d57a2dfd77 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Helena=20J=C3=A4ger?= Date: Wed, 20 Nov 2024 15:10:45 +0100 Subject: [PATCH 10/47] fixed 'dilate' typo --- .../bitbots_path_planning/bitbots_path_planning/map.py | 6 +++--- .../config/path_planning_parameters.yaml | 4 ++-- 2 files changed, 5 insertions(+), 5 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 index 14235473c..e9a02728f 100644 --- a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/map.py +++ b/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/map.py @@ -29,7 +29,7 @@ def __init__(self, node: Node, buffer: tf2.Buffer) -> None: self.frame: str = self.node.config.map.planning_frame self.config_ball_diameter: float = self.node.config.map.ball_diameter self.config_inflation_blur: int = self.node.config.map.inflation.blur - self.config_inflation_dialation: int = self.node.config.map.inflation.dialate + self.config_inflation_dilation: int = self.node.config.map.inflation.dilate self.config_obstacle_value: int = self.node.config.map.obstacle_value self.ball_obstacle_active: bool = True self._robot_geometries: list[Geometry] = [] @@ -87,7 +87,7 @@ def _update_robot_geometries(self, robots): print(robot) 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 + dilation = self.config_inflation_dilation / self.node.config.map.resolution geometry = center.buffer(radius + dilation, quad_segs=CIRCLE_APPROXIMATION_SEGMENTS // 4) self._robot_geometries.append(geometry) @@ -97,7 +97,7 @@ def _update_ball_geometries(self, balls): for ball in balls: center = shapely.Point(ball.x, ball.y) radius = self.config_ball_diameter / 2 - dilation = self.config_inflation_dialation / self.node.config.map.resolution + dilation = self.config_inflation_dilation / self.node.config.map.resolution geometry = center.buffer(radius + dilation, quad_segs=CIRCLE_APPROXIMATION_SEGMENTS // 4) self._ball_geometries.append(geometry) diff --git a/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml b/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml index 1f7c94211..b5e2a5480 100644 --- a/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml +++ b/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml @@ -59,10 +59,10 @@ bitbots_path_planning: description: 'The value of the obstacles in the map' read_only: true inflation: - dialate: + dilate: type: int default_value: 3 - description: 'The dialte value for the inflation' + description: 'The dilation value for the inflation' read_only: true blur: type: int From 0d509fb8e63f5d4e6b864214f3e22b565516a074 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Helena=20J=C3=A4ger?= Date: Wed, 20 Nov 2024 15:19:22 +0100 Subject: [PATCH 11/47] changed dilation parameter to map coordinates --- .../bitbots_path_planning/bitbots_path_planning/map.py | 4 ++-- .../config/path_planning_parameters.yaml | 4 ++-- 2 files changed, 4 insertions(+), 4 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 index e9a02728f..540248c67 100644 --- a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/map.py +++ b/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/map.py @@ -87,7 +87,7 @@ def _update_robot_geometries(self, robots): print(robot) 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_dilation / self.node.config.map.resolution + dilation = self.config_inflation_dilation geometry = center.buffer(radius + dilation, quad_segs=CIRCLE_APPROXIMATION_SEGMENTS // 4) self._robot_geometries.append(geometry) @@ -97,7 +97,7 @@ def _update_ball_geometries(self, balls): for ball in balls: center = shapely.Point(ball.x, ball.y) radius = self.config_ball_diameter / 2 - dilation = self.config_inflation_dilation / self.node.config.map.resolution + dilation = self.config_inflation_dilation geometry = center.buffer(radius + dilation, quad_segs=CIRCLE_APPROXIMATION_SEGMENTS // 4) self._ball_geometries.append(geometry) diff --git a/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml b/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml index b5e2a5480..422a1b312 100644 --- a/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml +++ b/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml @@ -60,8 +60,8 @@ bitbots_path_planning: read_only: true inflation: dilate: - type: int - default_value: 3 + type: double + default_value: 0.5 description: 'The dilation value for the inflation' read_only: true blur: From 1247d15c112ef7c18ee3835bb3b5870d6021b40c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Helena=20J=C3=A4ger?= Date: Wed, 20 Nov 2024 16:05:44 +0100 Subject: [PATCH 12/47] minor tweaks --- .../bitbots_path_planning/map.py | 23 +++++----- .../config/path_planning_parameters.yaml | 2 +- .../config/path_planning_viz.rviz | 43 +++++++++++++++++-- requirements/common.txt | 1 + 4 files changed, 54 insertions(+), 15 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 index 540248c67..c09f045f9 100644 --- a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/map.py +++ b/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/map.py @@ -29,7 +29,8 @@ def __init__(self, node: Node, buffer: tf2.Buffer) -> None: self.frame: str = self.node.config.map.planning_frame self.config_ball_diameter: float = self.node.config.map.ball_diameter self.config_inflation_blur: int = self.node.config.map.inflation.blur - self.config_inflation_dilation: int = self.node.config.map.inflation.dilate + self.config_inflation_dilation: float = self.node.config.map.inflation.dilate + print(f"dilation is {self.config_inflation_dilation}") self.config_obstacle_value: int = self.node.config.map.obstacle_value self.ball_obstacle_active: bool = True self._robot_geometries: list[Geometry] = [] @@ -40,7 +41,7 @@ def obstacles(self) -> list[Geometry]: """ Return the obstacles in the map as Geometry objects """ - return self._robot_geometries + self._ball_geometries + return self._robot_geometries + (self._ball_geometries if self.ball_obstacle_active else []) def intersects(self, object: Geometry) -> bool: """ @@ -93,22 +94,24 @@ def _update_robot_geometries(self, robots): def _update_ball_geometries(self, balls): self._ball_geometries = [] - if self.ball_obstacle_active: - for ball in balls: - center = shapely.Point(ball.x, ball.y) - radius = self.config_ball_diameter / 2 - dilation = self.config_inflation_dilation - geometry = center.buffer(radius + dilation, quad_segs=CIRCLE_APPROXIMATION_SEGMENTS // 4) - self._ball_geometries.append(geometry) + for ball in balls: + center = shapely.Point(ball.x, ball.y) + radius = self.config_ball_diameter / 2 + dilation = self.config_inflation_dilation + geometry = center.buffer(radius + dilation, quad_segs=CIRCLE_APPROXIMATION_SEGMENTS // 4) + self._ball_geometries.append(geometry) def _update_obstacle_union(self): - self._obstacle_union = shapely.union_all(self._ball_geometries + self._robot_geometries) + self._obstacle_union = shapely.union_all( + (self._ball_geometries if self.ball_obstacle_active else []) + self._robot_geometries + ) def avoid_ball(self, state: bool) -> None: """ Activates or deactivates the ball obstacle """ self.ball_obstacle_active = state + self._update_obstacle_union() def get_frame(self) -> str: """ diff --git a/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml b/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml index 422a1b312..c7d50e821 100644 --- a/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml +++ b/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml @@ -61,7 +61,7 @@ bitbots_path_planning: inflation: dilate: type: double - default_value: 0.5 + default_value: 0.15 description: 'The dilation value for the inflation' read_only: true blur: diff --git a/bitbots_navigation/bitbots_path_planning/config/path_planning_viz.rviz b/bitbots_navigation/bitbots_path_planning/config/path_planning_viz.rviz index 0a5fc51ed..cd686d48b 100644 --- a/bitbots_navigation/bitbots_path_planning/config/path_planning_viz.rviz +++ b/bitbots_navigation/bitbots_path_planning/config/path_planning_viz.rviz @@ -405,6 +405,41 @@ Visualization Manager: Reliability Policy: Reliable Value: /pose_with_covariance Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz_default_plugins/PoseWithCovariance + Color: 255; 25; 0 + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: PoseWithCovariance + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /ball_position_relative_filtered + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -450,7 +485,7 @@ Visualization Manager: Value: true Views: Current: - Angle: 1.579999566078186 + Angle: 3.0500032901763916 Class: rviz_default_plugins/TopDownOrtho Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 @@ -460,11 +495,11 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Scale: 108.03849029541016 + Scale: 133.8726348876953 Target Frame: Value: TopDownOrtho (rviz_default_plugins) - X: 0.3327471911907196 - Y: 0.0025227859150618315 + X: 0.6864694356918335 + Y: -1.4928948879241943 Saved: ~ Window Geometry: Displays: diff --git a/requirements/common.txt b/requirements/common.txt index 6f1f57acf..cb1fb1659 100644 --- a/requirements/common.txt +++ b/requirements/common.txt @@ -4,6 +4,7 @@ transforms3d==0.4.1 git+https://github.com/Flova/pyastar2d git+https://github.com/bit-bots/YOEO heapdict==1.0.1 +rustworkx==0.15.1 simpleeval # The following dependencies are only needed for rl walking and we don't actively use them currently From 5a807b2d27d120f9f85060b1b3f676a0be5b3bf2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Helena=20J=C3=A4ger?= Date: Wed, 20 Nov 2024 17:52:40 +0100 Subject: [PATCH 13/47] A* with rustworkx --- .../bitbots_path_planning/map.py | 2 - .../bitbots_path_planning/path_planning.py | 10 +++ .../bitbots_path_planning/planner.py | 86 ++++++++++++++++++- .../config/path_planning_parameters.yaml | 2 +- .../config/path_planning_viz.rviz | 38 +++++--- 5 files changed, 122 insertions(+), 16 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 index c09f045f9..0ade6b421 100644 --- a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/map.py +++ b/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/map.py @@ -30,7 +30,6 @@ def __init__(self, node: Node, buffer: tf2.Buffer) -> None: self.config_ball_diameter: float = self.node.config.map.ball_diameter self.config_inflation_blur: int = self.node.config.map.inflation.blur self.config_inflation_dilation: float = self.node.config.map.inflation.dilate - print(f"dilation is {self.config_inflation_dilation}") self.config_obstacle_value: int = self.node.config.map.obstacle_value self.ball_obstacle_active: bool = True self._robot_geometries: list[Geometry] = [] @@ -85,7 +84,6 @@ def set_robots(self, robots: sv3dm.RobotArray): def _update_robot_geometries(self, robots): self._robot_geometries = [] for robot in robots: - print(robot) 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_dilation 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 1afefea9f..c4c234c3c 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 @@ -1,3 +1,5 @@ +import time + import rclpy import soccer_vision_3d_msgs.msg as sv3dm from bitbots_tf_buffer import Buffer @@ -8,6 +10,7 @@ from rclpy.executors import MultiThreadedExecutor from rclpy.node import Node from std_msgs.msg import Bool, Empty +from visualization_msgs.msg import MarkerArray from bitbots_path_planning.controller import Controller from bitbots_path_planning.map import Map @@ -70,6 +73,7 @@ def __init__(self) -> None: self.cmd_vel_pub = self.create_publisher(Twist, "cmd_vel", 1) self.path_pub = self.create_publisher(Path, "path", 1) self.carrot_pub = self.create_publisher(PointStamped, "carrot", 1) + self.graph_pub = self.create_publisher(MarkerArray, "visibility_graph", 1) # Timer that updates the path and command velocity at a given rate self.create_timer( @@ -89,7 +93,13 @@ def step(self) -> None: try: if self.planner.active(): # Calculate the path to the goal pose considering the current map + t1 = time.time() path = self.planner.step() + t2 = time.time() + print(f"delta = {t2 - t1}") + # Publish the visibility graph for visualization + markers = self.planner.visibility_graph_wrapper() + self.graph_pub.publish(markers) # Publish the path for visualization self.path_pub.publish(path) # Calculate the command velocity to follow the given path 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 37efe3750..b5b8e4966 100644 --- a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/planner.py +++ b/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/planner.py @@ -1,14 +1,18 @@ +import typing from abc import ABC, abstractmethod +from itertools import product +import geometry_msgs.msg as geom_msg +import rustworkx as rx 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 visualization_msgs.msg import Marker, MarkerArray from bitbots_path_planning.map import Map @@ -73,6 +77,85 @@ def active(self) -> bool: """ return self.goal is not None + def visibility_graph(self, start: Point, goal: Point) -> typing.Tuple[rx.PyGraph, int, int]: + graph = rx.PyGraph() + start_node = graph.add_node(start) + goal_node = graph.add_node(goal) + for obstacle in self.map.obstacles(): + for x, y in obstacle.boundary.coords: + graph.add_node(Point(x, y)) + for a, b in product(graph.node_indices(), repeat=2): + if a == b: + continue + a_point = graph.get_node_data(a) + b_point = graph.get_node_data(b) + if not self.map.intersects(LineString([a_point, b_point])): + graph.add_edge(a, b, distance(a_point, b_point)) + return (graph, start_node, goal_node) + + def visibility_graph_wrapper(self) -> MarkerArray: + 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) + (graph, _, _) = self.visibility_graph(start, goal) + markers = MarkerArray() + nodes = Marker(type=Marker.POINTS, ns="visibility_graph_nodes", action=Marker.ADD) + nodes.header.frame_id = self.map.get_frame() + nodes.header.stamp = self.node.get_clock().now().to_msg() + for node in graph.nodes(): + nodes.points.append(geom_msg.Point(x=node.x, y=node.y, z=0.5)) + nodes.color.a = 1.0 + nodes.color.r = 0.2 + nodes.color.g = 0.2 + nodes.color.b = 0.2 + nodes.scale.x = 0.03 + nodes.scale.y = 0.03 + nodes.scale.z = 0.03 + markers.markers.append(nodes) + edges = Marker(type=Marker.LINE_LIST, ns="visibility_graph_edges", action=Marker.ADD) + edges.header.frame_id = self.map.get_frame() + edges.header.stamp = self.node.get_clock().now().to_msg() + for edge in graph.edge_indices(): + (a, b) = graph.get_edge_endpoints_by_index(edge) + a_point = graph.get_node_data(a) + b_point = graph.get_node_data(b) + edges.points.append(geom_msg.Point(x=a_point.x, y=a_point.y, z=0.5)) + edges.points.append(geom_msg.Point(x=b_point.x, y=b_point.y, z=0.5)) + edges.color.a = 1.0 + edges.color.r = 0.2 + edges.color.g = 0.2 + edges.color.b = 0.2 + edges.scale.x = 0.015 + markers.markers.append(edges) + return markers + + 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) + (graph, start_node, goal_node) = self.visibility_graph(start, goal) + path = rx.astar_shortest_path( + graph, start_node, lambda node: node == goal, lambda edge: edge, lambda node: distance(node, goal) + ) + + def map_to_pose(node: int): + position = graph.get_node_data(node) + pose = PoseStamped() + pose.pose.position.x = position.x + pose.pose.position.y = position.y + return pose + + return Path( + header=Header(frame_id=self.map.get_frame(), stamp=self.node.get_clock().now().to_msg()), + poses=list(map(map_to_pose, path)), + ) + + +""" def step(self) -> Path: goal = Point(self.goal.pose.position.x, self.goal.pose.position.y) my_position = self.buffer.lookup_transform( @@ -124,3 +207,4 @@ def step(self) -> Path: successor.cost = tentative_g f = tentative_g + distance(node.point, goal) # calculate new f-value (costs + heuristic) open_list[successor] = f +""" diff --git a/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml b/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml index c7d50e821..6e9ed58e9 100644 --- a/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml +++ b/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml @@ -61,7 +61,7 @@ bitbots_path_planning: inflation: dilate: type: double - default_value: 0.15 + default_value: 0.25 description: 'The dilation value for the inflation' read_only: true blur: diff --git a/bitbots_navigation/bitbots_path_planning/config/path_planning_viz.rviz b/bitbots_navigation/bitbots_path_planning/config/path_planning_viz.rviz index cd686d48b..070ec9c15 100644 --- a/bitbots_navigation/bitbots_path_planning/config/path_planning_viz.rviz +++ b/bitbots_navigation/bitbots_path_planning/config/path_planning_viz.rviz @@ -10,6 +10,7 @@ Panels: - /Map1 - /Map1/Topic1 - /Path1 + - /Path1/Offset1 - /Pose1 Splitter Ratio: 0.5 Tree Height: 1586 @@ -327,16 +328,16 @@ Visualization Manager: Class: rviz_default_plugins/Path Color: 129; 61; 156 Enabled: true - Head Diameter: 0.30000001192092896 + Head Diameter: 1 Head Length: 0.20000000298023224 Length: 0.30000001192092896 - Line Style: Lines + Line Style: Billboards Line Width: 0.029999999329447746 Name: Path Offset: X: 0 Y: 0 - Z: 0 + Z: 1 Pose Color: 255; 85; 255 Pose Style: None Radius: 0.029999999329447746 @@ -440,6 +441,19 @@ Visualization Manager: Reliability Policy: Reliable Value: /ball_position_relative_filtered Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + visibility_graph_edges: true + visibility_graph_nodes: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /visibility_graph + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -485,7 +499,7 @@ Visualization Manager: Value: true Views: Current: - Angle: 3.0500032901763916 + Angle: -1.3999996185302734 Class: rviz_default_plugins/TopDownOrtho Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 @@ -495,19 +509,19 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Scale: 133.8726348876953 + Scale: 166.31837463378906 Target Frame: Value: TopDownOrtho (rviz_default_plugins) - X: 0.6864694356918335 - Y: -1.4928948879241943 + X: 0 + Y: 0 Saved: ~ Window Geometry: Displays: - collapsed: true + collapsed: false Height: 1883 - Hide Left Dock: true - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000001b0000006bdfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d000006bd000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000006bdfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000006bd000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002fb00fffffffb0000000800540069006d006501000000000000045000000000000000000000039b000006bd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd0000000400000000000001b0000006bdfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000002d000000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000006bd000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000006bdfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000006bd000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002fa000006bd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -515,7 +529,7 @@ Window Geometry: Tool Properties: collapsed: false Views: - collapsed: false + collapsed: true Width: 1200 X: 1920 Y: 0 From 9c7a1452e0023ca12d9d5de0929e6bd3aa1b46d9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Helena=20J=C3=A4ger?= Date: Wed, 20 Nov 2024 18:13:15 +0100 Subject: [PATCH 14/47] path planning rviz graph update --- .../bitbots_path_planning/config/path_planning_viz.rviz | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/bitbots_navigation/bitbots_path_planning/config/path_planning_viz.rviz b/bitbots_navigation/bitbots_path_planning/config/path_planning_viz.rviz index 070ec9c15..892b7e6f0 100644 --- a/bitbots_navigation/bitbots_path_planning/config/path_planning_viz.rviz +++ b/bitbots_navigation/bitbots_path_planning/config/path_planning_viz.rviz @@ -499,7 +499,7 @@ Visualization Manager: Value: true Views: Current: - Angle: -1.3999996185302734 + Angle: -1.429999589920044 Class: rviz_default_plugins/TopDownOrtho Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 @@ -509,7 +509,7 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Scale: 166.31837463378906 + Scale: 77.23892974853516 Target Frame: Value: TopDownOrtho (rviz_default_plugins) X: 0 From 9d4d1becd6bfc12160c7da5d4acbb1347356b010 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Helena=20J=C3=A4ger?= Date: Wed, 11 Dec 2024 16:58:59 +0100 Subject: [PATCH 15/47] Merge with mafiasi path planning codebase --- Cargo.toml | 20 +++++ src/lib.rs | 13 +++ src/map.rs | 245 ++++++++++++++++++++++++++++++++++++++++++++++++++++ src/util.rs | 11 +++ test.py | 52 +++++++++++ 5 files changed, 341 insertions(+) create mode 100644 Cargo.toml create mode 100644 src/lib.rs create mode 100644 src/map.rs create mode 100644 src/util.rs create mode 100644 test.py diff --git a/Cargo.toml b/Cargo.toml new file mode 100644 index 000000000..d1cb2b74a --- /dev/null +++ b/Cargo.toml @@ -0,0 +1,20 @@ +[package] +name = "bbpprs" +version = "0.1.0" +edition = "2021" + +# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html +[lib] +name = "bbpprs" +crate-type = ["cdylib"] + +[dependencies] +pyo3 = "0.23.1" +anyhow = "1.0.93" +geo = "0.29.2" +itertools = "0.13.0" +petgraph = "0.6.5" +plotters = "0.3.7" +rand = "0.8.5" +keyed_priority_queue = "0.4.2" +ordered-float = "4.5.0" diff --git a/src/lib.rs b/src/lib.rs new file mode 100644 index 000000000..4100ce63f --- /dev/null +++ b/src/lib.rs @@ -0,0 +1,13 @@ +use map::{Ball, ObstacleMap, ObstacleMapConfig, Robot}; +use pyo3::prelude::*; +pub mod map; +pub mod util; + +#[pymodule] +fn bbpprs(m: &Bound<'_, PyModule>) -> PyResult<()> { + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + Ok(()) +} diff --git a/src/map.rs b/src/map.rs new file mode 100644 index 000000000..107ec4f68 --- /dev/null +++ b/src/map.rs @@ -0,0 +1,245 @@ +use std::collections::{HashMap, HashSet}; + +use geo::{ + BooleanOps, Coord, Distance, Euclidean, Intersects, Line, LineString, MultiPolygon, Polygon +}; +use keyed_priority_queue::{Entry, KeyedPriorityQueue}; +use ordered_float::OrderedFloat; +use pyo3::prelude::*; + +use crate::util::regular_ngon; + +/// A trait representing an Obstacle that can be represented as a Polygon +pub trait Obstacle { + /// Get the Polygon representation of this obstacle + fn as_polygon(&self, config: &ObstacleMapConfig) -> Polygon; + + /// Get the Polygon representation of this obstacle FOR INTERSECTION + fn as_polygon_for_intersection(&self, config: &ObstacleMapConfig) -> Polygon; +} + +/// A robot obstacle +#[pyclass(str = "Robot(center={center:?}, radius={radius})")] +#[derive(Debug, Copy, Clone)] +pub struct Robot { + #[pyo3(get, set)] + center: (f64, f64), + #[pyo3(get, set)] + radius: f64, +} + +#[pymethods] +impl Robot { + #[new] + pub fn new(center: (f64, f64), radius: f64) -> Self { + Self { center, radius } + } +} + +impl Obstacle for Robot { + fn as_polygon(&self, config: &ObstacleMapConfig) -> Polygon { + regular_ngon(self.center.into(), self.radius + config.dilate, 12) + } + + fn as_polygon_for_intersection(&self, config: &ObstacleMapConfig) -> Polygon { + regular_ngon(self.center.into(), self.radius + config.dilate - 0.01, 12) + } +} + +#[pyclass(str = "Robot(center={center:?}, radius={radius})")] +#[derive(Debug, Copy, Clone)] +pub struct Ball { + #[pyo3(get, set)] + center: (f64, f64), + #[pyo3(get, set)] + radius: f64, +} + +#[pymethods] +impl Ball { + #[new] + pub fn new(center: (f64, f64), radius: f64) -> Self { + Self { center, radius } + } +} + +impl Obstacle for Ball { + fn as_polygon(&self, config: &ObstacleMapConfig) -> Polygon { + regular_ngon(self.center.into(), self.radius + config.dilate, 12) + } + + fn as_polygon_for_intersection(&self, config: &ObstacleMapConfig) -> Polygon { + regular_ngon(self.center.into(), self.radius + config.dilate - 0.01, 12) + } +} +#[pyclass] +#[derive(Debug, Clone, Copy)] +pub struct ObstacleMapConfig { + #[pyo3(get, set)] + dilate: f64, +} + +#[pymethods] +impl ObstacleMapConfig { + #[new] + pub fn new(dilate: f64) -> Self { + Self { dilate } + } +} + +#[pyclass] +#[derive(Debug, Clone)] +pub struct ObstacleMap { + config: ObstacleMapConfig, + #[pyo3(get, set)] + ball: Option, + #[pyo3(get, set)] + robots: Vec, +} + +impl ObstacleMap{ + /// Get the list of polygonal obstacles in the map + fn polygons(&self) -> Vec { + let mut obstacles = self + .robots + .iter() + .map(|robot| robot.as_polygon(&self.config)) + .collect::>(); + if let Some(ball) = self.ball { + obstacles.push(ball.as_polygon(&self.config)); + }; + obstacles + } + + /// Get the list of polygonal obstacles in the map for correct intersection + fn polygons_for_intersection(&self) -> Vec { + let mut obstacles = self + .robots + .iter() + .map(|robot| robot.as_polygon_for_intersection(&self.config)) + .collect::>(); + if let Some(ball) = self.ball { + obstacles.push(ball.as_polygon_for_intersection(&self.config)); + }; + obstacles + } + + /// Get the visibility graph of this map + fn vertices_and_obstacles(&self, start: Coord, goal: Coord) -> (Vec, MultiPolygon) { + let polygons = self.polygons(); + let polygons_for_intersection = self.polygons_for_intersection(); + let vertices = polygons + .iter() + .map(|polygon| polygon.exterior().coords().cloned()) + .flatten() + .chain(vec![start, goal].into_iter()) + .collect::>(); + let obstacles = polygons_for_intersection.iter().fold( + MultiPolygon::new(vec![Polygon::new(LineString::new(vec![]), vec![])]), + |a, b| a.union(b), + ); + (vertices, obstacles) + } + + fn heuristic(vertices: &Vec, node: usize) -> OrderedFloat { + OrderedFloat(Euclidean::distance( + vertices[node], + vertices[vertices.len() - 1], + )) + } + + fn distance(vertices: &Vec, from: usize, to: usize) -> OrderedFloat { + OrderedFloat(Euclidean::distance(vertices[from], vertices[to])) + } +} + +#[pymethods] +impl ObstacleMap { + /// Create a new ObstacleMap with the given initial ball and robots + #[new] + #[pyo3(signature=(config, robots, ball=None))] + pub fn new(config: ObstacleMapConfig, robots: Vec, ball: Option) -> Self { + Self { + config, + ball, + robots, + } + } + + /// Get the shortest from start to goal in a given visibilty graph, which defaults to self.visibility_graph + pub fn shortest_path(&self, start: (f64, f64), goal: (f64, f64)) -> Option> { + let (vertices, obstacles) = self.vertices_and_obstacles(start.into(), goal.into()); + let mut successors = HashSet::::from_iter(0..vertices.len()); + let mut open = KeyedPriorityQueue::>::new(); + open.push( + vertices.len() - 2, + -Self::heuristic(&vertices, vertices.len() - 2), + ); + let mut from = HashMap::::new(); + let mut g_score = HashMap::>::new(); + g_score.insert(vertices.len() - 2, OrderedFloat(0.0)); + + // needed if goal is in obstacle + let mut closest_vertex = vertices.len() - 2; + let mut closest_distance = OrderedFloat(std::f64::INFINITY); + + while let Some((vertex, _)) = open.pop() { + + if Self::heuristic(&vertices, vertex) < closest_distance - 0.01 + { + closest_vertex = vertex; + closest_distance = Self::heuristic(&vertices, vertex); + }; + + if vertex == vertices.len() - 1 { + let mut path = vec![vertex]; + let mut current = vertex; + while let Some(previous) = from.get(¤t) { + current = *previous; + path.push(*previous); + } + path.reverse(); + return Some(path.into_iter().map(|idx| vertices[idx].x_y()).collect()); + } + let g_vertex = g_score + .get(&vertex) + .unwrap_or(&OrderedFloat(std::f64::INFINITY)) + .clone(); + successors.remove(&vertex); + //println!("evaluating successors to node {} (coords={:?})", vertex, vertices[vertex]); + for successor in successors.iter() { + if obstacles.intersects(&Line::new(vertices[vertex], vertices[*successor])) { + //println!(" not considering node {} (coords={:?}) because of intersection", successor, vertices[*successor]); + continue; + } + //println!(" successor={} coords={:?}", successor, vertices[*successor]); + let g_successor = g_score + .get(successor) + .unwrap_or(&OrderedFloat(std::f64::INFINITY)); + let g_tentative = g_vertex + &Self::distance(&vertices, vertex, *successor); + //println!(" successor={successor}, tentative={g_tentative}, successor={g_successor}"); + if g_tentative < *g_successor { + from.insert(*successor, vertex); + g_score.insert(*successor, g_tentative); + match open.entry(*successor) { + Entry::Occupied(entry) => { + entry.set_priority(-(g_tentative + Self::heuristic(&vertices, *successor))); + }, + Entry::Vacant(entry) => { + entry.set_priority(-(g_tentative + Self::heuristic(&vertices, *successor))); + } + }; + }; + } + } + let mut path = vec![vertices.len() - 1]; + let mut current = closest_vertex; + path.push(current); + while let Some(previous) = from.get(¤t) { + current = *previous; + path.push(*previous); + } + path.reverse(); + return Some(path.into_iter().map(|idx| vertices[idx].x_y()).collect()); + } +} diff --git a/src/util.rs b/src/util.rs new file mode 100644 index 000000000..d17308847 --- /dev/null +++ b/src/util.rs @@ -0,0 +1,11 @@ +use geo::{Coord, LineString, Polygon}; + +pub fn regular_ngon(center: Coord, radius: f64, sides: usize) -> Polygon { + let points = (0..sides) + .map(|side| { + let angle = (side as f64) / (sides as f64) * std::f64::consts::TAU; + center + Coord::from((radius * angle.cos(), radius * angle.sin())) + }) + .collect::>(); + Polygon::new(LineString::new(points.into()), vec![]) +} \ No newline at end of file diff --git a/test.py b/test.py new file mode 100644 index 000000000..c8c87d6a5 --- /dev/null +++ b/test.py @@ -0,0 +1,52 @@ +from random import uniform + +import matplotlib +import matplotlib.patches +from bbpprs import ObstacleMap, ObstacleMapConfig, Robot +from matplotlib import pyplot as plt + +config = ObstacleMapConfig(dilate=0.1) + + +def random_omap(config, obstacles): + l = [] + for _ in range(obstacles): + x = uniform(1.0, 9.0) + y = uniform(1.0, 9.0) + r = uniform(0.2, 2.0) + l.append(Robot((x, y), r)) + return ObstacleMap(config, l, None) + + +def debug_omap(config): + l = [] + l.append(Robot((1.0, 4.0), 1.0)) + l.append(Robot((4.2, 4.2), 1.0)) + l.append(Robot((5.0, 2.0), 1.4)) + l.append(Robot((10.0, 10.0), 0.5)) + return ObstacleMap(config, l, None) + + +""" +def random_omaps(config, n): + l = [] + for _ in range(n): + l.append(random_omap(config, 3)) + return l + +omaps = random_omaps(config, 100) + +_ = [om.shortest_path(start=(0.0, 0.0), goal=(10.0, 10.0)) for om in omaps] +""" + +omap = debug_omap(config) +path = omap.shortest_path(start=(0.0, 0.0), goal=(10.0, 10.0)) +print(path) +fig, ax = plt.subplots(1) +ax.axis([0, 11, 0, 11]) +for obstacle in omap.robots: + ax.add_patch(matplotlib.patches.RegularPolygon(obstacle.center, 12, radius=obstacle.radius)) +ax.plot([i[0] for i in path], [i[1] for i in path]) +print(str(omap.ball), [str(bot) for bot in omap.robots]) +# plt.plot(path) +plt.show() From 04852a7b126820de27fbee6a80e29a6a024a7881 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Helena=20J=C3=A4ger?= Date: Wed, 11 Dec 2024 17:01:47 +0100 Subject: [PATCH 16/47] Move files to correct location --- .../bitbots_path_planning/Cargo.lock | 1444 +++++++++++++++++ .../bitbots_path_planning/Cargo.toml | 0 .../bitbots_path_planning/src}/lib.rs | 0 .../bitbots_path_planning/src}/map.rs | 0 .../bitbots_path_planning/src}/util.rs | 0 .../bitbots_path_planning/test/test.py | 0 6 files changed, 1444 insertions(+) create mode 100644 bitbots_navigation/bitbots_path_planning/Cargo.lock rename Cargo.toml => bitbots_navigation/bitbots_path_planning/Cargo.toml (100%) rename {src => bitbots_navigation/bitbots_path_planning/src}/lib.rs (100%) rename {src => bitbots_navigation/bitbots_path_planning/src}/map.rs (100%) rename {src => bitbots_navigation/bitbots_path_planning/src}/util.rs (100%) rename test.py => bitbots_navigation/bitbots_path_planning/test/test.py (100%) diff --git a/bitbots_navigation/bitbots_path_planning/Cargo.lock b/bitbots_navigation/bitbots_path_planning/Cargo.lock new file mode 100644 index 000000000..2d3bfec03 --- /dev/null +++ b/bitbots_navigation/bitbots_path_planning/Cargo.lock @@ -0,0 +1,1444 @@ +# This file is automatically @generated by Cargo. +# It is not intended for manual editing. +version = 3 + +[[package]] +name = "adler2" +version = "2.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "512761e0bb2578dd7380c6baaa0f4ce03e84f95e960231d1dec8bf4d7d6e2627" + +[[package]] +name = "ahash" +version = "0.8.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e89da841a80418a9b391ebaea17f5c112ffaaa96f621d2c285b5174da76b9011" +dependencies = [ + "cfg-if", + "once_cell", + "version_check", + "zerocopy", +] + +[[package]] +name = "allocator-api2" +version = "0.2.21" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "683d7910e743518b0e34f1186f92494becacb047c7b6bf616c96772180fef923" + +[[package]] +name = "android-tzdata" +version = "0.1.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e999941b234f3131b00bc13c22d06e8c5ff726d1b6318ac7eb276997bbb4fef0" + +[[package]] +name = "android_system_properties" +version = "0.1.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "819e7219dbd41043ac279b19830f2efc897156490d7fd6ea916720117ee66311" +dependencies = [ + "libc", +] + +[[package]] +name = "anyhow" +version = "1.0.94" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c1fd03a028ef38ba2276dce7e33fcd6369c158a1bca17946c4b1b701891c1ff7" + +[[package]] +name = "approx" +version = "0.5.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cab112f0a86d568ea0e627cc1d6be74a1e9cd55214684db5561995f6dad897c6" +dependencies = [ + "num-traits", +] + +[[package]] +name = "autocfg" +version = "1.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ace50bade8e6234aa140d9a2f552bbee1db4d353f69b8217bc503490fc1a9f26" + +[[package]] +name = "bbpprs" +version = "0.1.0" +dependencies = [ + "anyhow", + "geo", + "itertools 0.13.0", + "keyed_priority_queue", + "ordered-float", + "petgraph", + "plotters", + "pyo3", + "rand", +] + +[[package]] +name = "bitflags" +version = "1.3.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bef38d45163c2f1dde094a7dfd33ccf595c92905c8f8f4fdc18d06fb1037718a" + +[[package]] +name = "bitflags" +version = "2.6.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b048fb63fd8b5923fc5aa7b340d8e156aec7ec02f0c78fa8a6ddc2613f6f71de" + +[[package]] +name = "bumpalo" +version = "3.16.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "79296716171880943b8470b5f8d03aa55eb2e645a4874bdbb28adb49162e012c" + +[[package]] +name = "bytemuck" +version = "1.20.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8b37c88a63ffd85d15b406896cc343916d7cf57838a847b3a6f2ca5d39a5695a" + +[[package]] +name = "byteorder" +version = "1.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1fd0f2584146f6f2ef48085050886acf353beff7305ebd1ae69500e27c67f64b" + +[[package]] +name = "cc" +version = "1.2.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "27f657647bcff5394bf56c7317665bbf790a137a50eaaa5c6bfbb9e27a518f2d" +dependencies = [ + "shlex", +] + +[[package]] +name = "cfg-if" +version = "1.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "baf1de4339761588bc0619e3cbc0120ee582ebb74b53b4efbf79117bd2da40fd" + +[[package]] +name = "chrono" +version = "0.4.39" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7e36cc9d416881d2e24f9a963be5fb1cd90966419ac844274161d10488b3e825" +dependencies = [ + "android-tzdata", + "iana-time-zone", + "js-sys", + "num-traits", + "wasm-bindgen", + "windows-targets 0.52.6", +] + +[[package]] +name = "color_quant" +version = "1.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3d7b894f5411737b7867f4827955924d7c254fc9f4d91a6aad6b097804b1018b" + +[[package]] +name = "core-foundation" +version = "0.9.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "91e195e091a93c46f7102ec7818a2aa394e1e1771c3ab4825963fa03e45afb8f" +dependencies = [ + "core-foundation-sys", + "libc", +] + +[[package]] +name = "core-foundation-sys" +version = "0.8.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "773648b94d0e5d620f64f280777445740e61fe701025087ec8b57f45c791888b" + +[[package]] +name = "core-graphics" +version = "0.23.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c07782be35f9e1140080c6b96f0d44b739e2278479f64e02fdab4e32dfd8b081" +dependencies = [ + "bitflags 1.3.2", + "core-foundation", + "core-graphics-types", + "foreign-types", + "libc", +] + +[[package]] +name = "core-graphics-types" +version = "0.1.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "45390e6114f68f718cc7a830514a96f903cccd70d02a8f6d9f643ac4ba45afaf" +dependencies = [ + "bitflags 1.3.2", + "core-foundation", + "libc", +] + +[[package]] +name = "core-text" +version = "20.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c9d2790b5c08465d49f8dc05c8bcae9fea467855947db39b0f8145c091aaced5" +dependencies = [ + "core-foundation", + "core-graphics", + "foreign-types", + "libc", +] + +[[package]] +name = "crc32fast" +version = "1.4.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a97769d94ddab943e4510d138150169a2758b5ef3eb191a9ee688de3e23ef7b3" +dependencies = [ + "cfg-if", +] + +[[package]] +name = "crossbeam-deque" +version = "0.8.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "613f8cc01fe9cf1a3eb3d7f488fd2fa8388403e97039e2f73692932e291a770d" +dependencies = [ + "crossbeam-epoch", + "crossbeam-utils", +] + +[[package]] +name = "crossbeam-epoch" +version = "0.9.18" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5b82ac4a3c2ca9c3460964f020e1402edd5753411d7737aa39c3714ad1b5420e" +dependencies = [ + "crossbeam-utils", +] + +[[package]] +name = "crossbeam-utils" +version = "0.8.20" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "22ec99545bb0ed0ea7bb9b8e1e9122ea386ff8a48c0922e43f36d45ab09e0e80" + +[[package]] +name = "dirs" +version = "5.0.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "44c45a9d03d6676652bcb5e724c7e988de1acad23a711b5217ab9cbecbec2225" +dependencies = [ + "dirs-sys", +] + +[[package]] +name = "dirs-sys" +version = "0.4.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "520f05a5cbd335fae5a99ff7a6ab8627577660ee5cfd6a94a6a929b52ff0321c" +dependencies = [ + "libc", + "option-ext", + "redox_users", + "windows-sys 0.48.0", +] + +[[package]] +name = "dlib" +version = "0.5.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "330c60081dcc4c72131f8eb70510f1ac07223e5d4163db481a04a0befcffa412" +dependencies = [ + "libloading", +] + +[[package]] +name = "dwrote" +version = "0.11.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "70182709525a3632b2ba96b6569225467b18ecb4a77f46d255f713a6bebf05fd" +dependencies = [ + "lazy_static", + "libc", + "winapi", + "wio", +] + +[[package]] +name = "earcutr" +version = "0.4.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "79127ed59a85d7687c409e9978547cffb7dc79675355ed22da6b66fd5f6ead01" +dependencies = [ + "itertools 0.11.0", + "num-traits", +] + +[[package]] +name = "either" +version = "1.13.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "60b1af1c220855b6ceac025d3f6ecdd2b7c4894bfe9cd9bda4fbb4bc7c0d4cf0" + +[[package]] +name = "equivalent" +version = "1.0.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5443807d6dff69373d433ab9ef5378ad8df50ca6298caf15de6e52e24aaf54d5" + +[[package]] +name = "fdeflate" +version = "0.3.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1e6853b52649d4ac5c0bd02320cddc5ba956bdb407c4b75a2c6b75bf51500f8c" +dependencies = [ + "simd-adler32", +] + +[[package]] +name = "fixedbitset" +version = "0.4.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0ce7134b9999ecaf8bcd65542e436736ef32ddca1b3e06094cb6ec5755203b80" + +[[package]] +name = "flate2" +version = "1.0.35" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c936bfdafb507ebbf50b8074c54fa31c5be9a1e7e5f467dd659697041407d07c" +dependencies = [ + "crc32fast", + "miniz_oxide", +] + +[[package]] +name = "float-ord" +version = "0.3.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8ce81f49ae8a0482e4c55ea62ebbd7e5a686af544c00b9d090bba3ff9be97b3d" + +[[package]] +name = "float_next_after" +version = "1.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8bf7cc16383c4b8d58b9905a8509f02926ce3058053c056376248d958c9df1e8" + +[[package]] +name = "font-kit" +version = "0.14.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b64b34f4efd515f905952d91bc185039863705592c0c53ae6d979805dd154520" +dependencies = [ + "bitflags 2.6.0", + "byteorder", + "core-foundation", + "core-graphics", + "core-text", + "dirs", + "dwrote", + "float-ord", + "freetype-sys", + "lazy_static", + "libc", + "log", + "pathfinder_geometry", + "pathfinder_simd", + "walkdir", + "winapi", + "yeslogic-fontconfig-sys", +] + +[[package]] +name = "foreign-types" +version = "0.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d737d9aa519fb7b749cbc3b962edcf310a8dd1f4b67c91c4f83975dbdd17d965" +dependencies = [ + "foreign-types-macros", + "foreign-types-shared", +] + +[[package]] +name = "foreign-types-macros" +version = "0.2.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1a5c6c585bc94aaf2c7b51dd4c2ba22680844aba4c687be581871a6f518c5742" +dependencies = [ + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "foreign-types-shared" +version = "0.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "aa9a19cbb55df58761df49b23516a86d432839add4af60fc256da840f66ed35b" + +[[package]] +name = "freetype-sys" +version = "0.20.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0e7edc5b9669349acfda99533e9e0bcf26a51862ab43b08ee7745c55d28eb134" +dependencies = [ + "cc", + "libc", + "pkg-config", +] + +[[package]] +name = "geo" +version = "0.29.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "34f0e6e028c581e82e6822a68869514e94c25e7f8ea669a2d8595bdf7461ccc5" +dependencies = [ + "earcutr", + "float_next_after", + "geo-types", + "geographiclib-rs", + "i_overlay", + "log", + "num-traits", + "robust", + "rstar", + "spade", +] + +[[package]] +name = "geo-types" +version = "0.7.14" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b6f47c611187777bbca61ea7aba780213f5f3441fd36294ab333e96cfa791b65" +dependencies = [ + "approx", + "num-traits", + "rayon", + "rstar", + "serde", +] + +[[package]] +name = "geographiclib-rs" +version = "0.2.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e6e5ed84f8089c70234b0a8e0aedb6dc733671612ddc0d37c6066052f9781960" +dependencies = [ + "libm", +] + +[[package]] +name = "getrandom" +version = "0.2.15" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c4567c8db10ae91089c99af84c68c38da3ec2f087c3f82960bcdbf3656b6f4d7" +dependencies = [ + "cfg-if", + "libc", + "wasi", +] + +[[package]] +name = "gif" +version = "0.12.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "80792593675e051cf94a4b111980da2ba60d4a83e43e0048c5693baab3977045" +dependencies = [ + "color_quant", + "weezl", +] + +[[package]] +name = "hash32" +version = "0.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "47d60b12902ba28e2730cd37e95b8c9223af2808df9e902d4df49588d1470606" +dependencies = [ + "byteorder", +] + +[[package]] +name = "hashbrown" +version = "0.14.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e5274423e17b7c9fc20b6e7e208532f9b19825d82dfd615708b70edd83df41f1" +dependencies = [ + "ahash", + "allocator-api2", +] + +[[package]] +name = "hashbrown" +version = "0.15.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bf151400ff0baff5465007dd2f3e717f3fe502074ca563069ce3a6629d07b289" + +[[package]] +name = "heapless" +version = "0.8.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0bfb9eb618601c89945a70e254898da93b13be0388091d42117462b265bb3fad" +dependencies = [ + "hash32", + "stable_deref_trait", +] + +[[package]] +name = "heck" +version = "0.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2304e00983f87ffb38b55b444b5e3b60a884b5d30c0fca7d82fe33449bbe55ea" + +[[package]] +name = "i_float" +version = "1.6.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "775f9961a8d2f879725da8aff789bb20a3ddf297473e0c90af75e69313919490" +dependencies = [ + "serde", +] + +[[package]] +name = "i_key_sort" +version = "0.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "347c253b4748a1a28baf94c9ce133b6b166f08573157e05afe718812bc599fcd" + +[[package]] +name = "i_overlay" +version = "1.9.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "06740cd31c1f963823e007d8e6edcd2db634b2856f4f613e3df01737fd852482" +dependencies = [ + "i_float", + "i_key_sort", + "i_shape", + "i_tree", + "rayon", +] + +[[package]] +name = "i_shape" +version = "1.6.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "27dbe9e5238d6b9c694c08415bf00fb370b089949bd818ab01f41f8927b8774c" +dependencies = [ + "i_float", + "serde", +] + +[[package]] +name = "i_tree" +version = "0.8.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "155181bc97d770181cf9477da51218a19ee92a8e5be642e796661aee2b601139" + +[[package]] +name = "iana-time-zone" +version = "0.1.61" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "235e081f3925a06703c2d0117ea8b91f042756fd6e7a6e5d901e8ca1a996b220" +dependencies = [ + "android_system_properties", + "core-foundation-sys", + "iana-time-zone-haiku", + "js-sys", + "wasm-bindgen", + "windows-core", +] + +[[package]] +name = "iana-time-zone-haiku" +version = "0.1.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f31827a206f56af32e590ba56d5d2d085f558508192593743f16b2306495269f" +dependencies = [ + "cc", +] + +[[package]] +name = "image" +version = "0.24.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5690139d2f55868e080017335e4b94cb7414274c74f1669c84fb5feba2c9f69d" +dependencies = [ + "bytemuck", + "byteorder", + "color_quant", + "jpeg-decoder", + "num-traits", + "png", +] + +[[package]] +name = "indexmap" +version = "2.7.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "62f822373a4fe84d4bb149bf54e584a7f4abec90e072ed49cda0edea5b95471f" +dependencies = [ + "equivalent", + "hashbrown 0.15.2", +] + +[[package]] +name = "indoc" +version = "2.0.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b248f5224d1d606005e02c97f5aa4e88eeb230488bcc03bc9ca4d7991399f2b5" + +[[package]] +name = "itertools" +version = "0.11.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b1c173a5686ce8bfa551b3563d0c2170bf24ca44da99c7ca4bfdab5418c3fe57" +dependencies = [ + "either", +] + +[[package]] +name = "itertools" +version = "0.13.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "413ee7dfc52ee1a4949ceeb7dbc8a33f2d6c088194d9f922fb8318faf1f01186" +dependencies = [ + "either", +] + +[[package]] +name = "jpeg-decoder" +version = "0.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f5d4a7da358eff58addd2877a45865158f0d78c911d43a5784ceb7bbf52833b0" + +[[package]] +name = "js-sys" +version = "0.3.76" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6717b6b5b077764fb5966237269cb3c64edddde4b14ce42647430a78ced9e7b7" +dependencies = [ + "once_cell", + "wasm-bindgen", +] + +[[package]] +name = "keyed_priority_queue" +version = "0.4.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4ee7893dab2e44ae5f9d0173f26ff4aa327c10b01b06a72b52dd9405b628640d" +dependencies = [ + "indexmap", +] + +[[package]] +name = "lazy_static" +version = "1.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bbd2bcb4c963f2ddae06a2efc7e9f3591312473c50c6685e1f298068316e66fe" + +[[package]] +name = "libc" +version = "0.2.168" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5aaeb2981e0606ca11d79718f8bb01164f1d6ed75080182d3abf017e6d244b6d" + +[[package]] +name = "libloading" +version = "0.8.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "fc2f4eb4bc735547cfed7c0a4922cbd04a4655978c09b54f1f7b228750664c34" +dependencies = [ + "cfg-if", + "windows-targets 0.52.6", +] + +[[package]] +name = "libm" +version = "0.2.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8355be11b20d696c8f18f6cc018c4e372165b1fa8126cef092399c9951984ffa" + +[[package]] +name = "libredox" +version = "0.1.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c0ff37bd590ca25063e35af745c343cb7a0271906fb7b37e4813e8f79f00268d" +dependencies = [ + "bitflags 2.6.0", + "libc", +] + +[[package]] +name = "log" +version = "0.4.22" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a7a70ba024b9dc04c27ea2f0c0548feb474ec5c54bba33a7f72f873a39d07b24" + +[[package]] +name = "memoffset" +version = "0.9.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "488016bfae457b036d996092f6cb448677611ce4449e970ceaf42695203f218a" +dependencies = [ + "autocfg", +] + +[[package]] +name = "miniz_oxide" +version = "0.8.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e2d80299ef12ff69b16a84bb182e3b9df68b5a91574d3d4fa6e41b65deec4df1" +dependencies = [ + "adler2", + "simd-adler32", +] + +[[package]] +name = "num-traits" +version = "0.2.19" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "071dfc062690e90b734c0b2273ce72ad0ffa95f0c74596bc250dcfd960262841" +dependencies = [ + "autocfg", + "libm", +] + +[[package]] +name = "once_cell" +version = "1.20.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1261fe7e33c73b354eab43b1273a57c8f967d0391e80353e51f764ac02cf6775" + +[[package]] +name = "option-ext" +version = "0.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "04744f49eae99ab78e0d5c0b603ab218f515ea8cfe5a456d7629ad883a3b6e7d" + +[[package]] +name = "ordered-float" +version = "4.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c65ee1f9701bf938026630b455d5315f490640234259037edb259798b3bcf85e" +dependencies = [ + "num-traits", +] + +[[package]] +name = "pathfinder_geometry" +version = "0.5.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0b7b7e7b4ea703700ce73ebf128e1450eb69c3a8329199ffbfb9b2a0418e5ad3" +dependencies = [ + "log", + "pathfinder_simd", +] + +[[package]] +name = "pathfinder_simd" +version = "0.5.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5cf07ef4804cfa9aea3b04a7bbdd5a40031dbb6b4f2cbaf2b011666c80c5b4f2" +dependencies = [ + "rustc_version", +] + +[[package]] +name = "petgraph" +version = "0.6.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b4c5cc86750666a3ed20bdaf5ca2a0344f9c67674cae0515bec2da16fbaa47db" +dependencies = [ + "fixedbitset", + "indexmap", +] + +[[package]] +name = "pkg-config" +version = "0.3.31" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "953ec861398dccce10c670dfeaf3ec4911ca479e9c02154b3a215178c5f566f2" + +[[package]] +name = "plotters" +version = "0.3.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5aeb6f403d7a4911efb1e33402027fc44f29b5bf6def3effcc22d7bb75f2b747" +dependencies = [ + "chrono", + "font-kit", + "image", + "lazy_static", + "num-traits", + "pathfinder_geometry", + "plotters-backend", + "plotters-bitmap", + "plotters-svg", + "ttf-parser", + "wasm-bindgen", + "web-sys", +] + +[[package]] +name = "plotters-backend" +version = "0.3.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "df42e13c12958a16b3f7f4386b9ab1f3e7933914ecea48da7139435263a4172a" + +[[package]] +name = "plotters-bitmap" +version = "0.3.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "72ce181e3f6bf82d6c1dc569103ca7b1bd964c60ba03d7e6cdfbb3e3eb7f7405" +dependencies = [ + "gif", + "image", + "plotters-backend", +] + +[[package]] +name = "plotters-svg" +version = "0.3.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "51bae2ac328883f7acdfea3d66a7c35751187f870bc81f94563733a154d7a670" +dependencies = [ + "plotters-backend", +] + +[[package]] +name = "png" +version = "0.17.15" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b67582bd5b65bdff614270e2ea89a1cf15bef71245cc1e5f7ea126977144211d" +dependencies = [ + "bitflags 1.3.2", + "crc32fast", + "fdeflate", + "flate2", + "miniz_oxide", +] + +[[package]] +name = "portable-atomic" +version = "1.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "280dc24453071f1b63954171985a0b0d30058d287960968b9b2aca264c8d4ee6" + +[[package]] +name = "ppv-lite86" +version = "0.2.20" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "77957b295656769bb8ad2b6a6b09d897d94f05c41b069aede1fcdaa675eaea04" +dependencies = [ + "zerocopy", +] + +[[package]] +name = "proc-macro2" +version = "1.0.92" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "37d3544b3f2748c54e147655edb5025752e2303145b5aefb3c3ea2c78b973bb0" +dependencies = [ + "unicode-ident", +] + +[[package]] +name = "pyo3" +version = "0.23.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e484fd2c8b4cb67ab05a318f1fd6fa8f199fcc30819f08f07d200809dba26c15" +dependencies = [ + "cfg-if", + "indoc", + "libc", + "memoffset", + "once_cell", + "portable-atomic", + "pyo3-build-config", + "pyo3-ffi", + "pyo3-macros", + "unindent", +] + +[[package]] +name = "pyo3-build-config" +version = "0.23.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dc0e0469a84f208e20044b98965e1561028180219e35352a2afaf2b942beff3b" +dependencies = [ + "once_cell", + "target-lexicon", +] + +[[package]] +name = "pyo3-ffi" +version = "0.23.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "eb1547a7f9966f6f1a0f0227564a9945fe36b90da5a93b3933fc3dc03fae372d" +dependencies = [ + "libc", + "pyo3-build-config", +] + +[[package]] +name = "pyo3-macros" +version = "0.23.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "fdb6da8ec6fa5cedd1626c886fc8749bdcbb09424a86461eb8cdf096b7c33257" +dependencies = [ + "proc-macro2", + "pyo3-macros-backend", + "quote", + "syn", +] + +[[package]] +name = "pyo3-macros-backend" +version = "0.23.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "38a385202ff5a92791168b1136afae5059d3ac118457bb7bc304c197c2d33e7d" +dependencies = [ + "heck", + "proc-macro2", + "pyo3-build-config", + "quote", + "syn", +] + +[[package]] +name = "quote" +version = "1.0.37" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b5b9d34b8991d19d98081b46eacdd8eb58c6f2b201139f7c5f643cc155a633af" +dependencies = [ + "proc-macro2", +] + +[[package]] +name = "rand" +version = "0.8.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "34af8d1a0e25924bc5b7c43c079c942339d8f0a8b57c39049bef581b46327404" +dependencies = [ + "libc", + "rand_chacha", + "rand_core", +] + +[[package]] +name = "rand_chacha" +version = "0.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e6c10a63a0fa32252be49d21e7709d4d4baf8d231c2dbce1eaa8141b9b127d88" +dependencies = [ + "ppv-lite86", + "rand_core", +] + +[[package]] +name = "rand_core" +version = "0.6.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ec0be4795e2f6a28069bec0b5ff3e2ac9bafc99e6a9a7dc3547996c5c816922c" +dependencies = [ + "getrandom", +] + +[[package]] +name = "rayon" +version = "1.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b418a60154510ca1a002a752ca9714984e21e4241e804d32555251faf8b78ffa" +dependencies = [ + "either", + "rayon-core", +] + +[[package]] +name = "rayon-core" +version = "1.12.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1465873a3dfdaa8ae7cb14b4383657caab0b3e8a0aa9ae8e04b044854c8dfce2" +dependencies = [ + "crossbeam-deque", + "crossbeam-utils", +] + +[[package]] +name = "redox_users" +version = "0.4.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ba009ff324d1fc1b900bd1fdb31564febe58a8ccc8a6fdbb93b543d33b13ca43" +dependencies = [ + "getrandom", + "libredox", + "thiserror", +] + +[[package]] +name = "robust" +version = "1.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cbf4a6aa5f6d6888f39e980649f3ad6b666acdce1d78e95b8a2cb076e687ae30" + +[[package]] +name = "rstar" +version = "0.12.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "421400d13ccfd26dfa5858199c30a5d76f9c54e0dba7575273025b43c5175dbb" +dependencies = [ + "heapless", + "num-traits", + "smallvec", +] + +[[package]] +name = "rustc_version" +version = "0.4.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cfcb3a22ef46e85b45de6ee7e79d063319ebb6594faafcf1c225ea92ab6e9b92" +dependencies = [ + "semver", +] + +[[package]] +name = "same-file" +version = "1.0.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "93fc1dc3aaa9bfed95e02e6eadabb4baf7e3078b0bd1b4d7b6b0b68378900502" +dependencies = [ + "winapi-util", +] + +[[package]] +name = "semver" +version = "1.0.23" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "61697e0a1c7e512e84a621326239844a24d8207b4669b41bc18b32ea5cbf988b" + +[[package]] +name = "serde" +version = "1.0.216" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0b9781016e935a97e8beecf0c933758c97a5520d32930e460142b4cd80c6338e" +dependencies = [ + "serde_derive", +] + +[[package]] +name = "serde_derive" +version = "1.0.216" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "46f859dbbf73865c6627ed570e78961cd3ac92407a2d117204c49232485da55e" +dependencies = [ + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "shlex" +version = "1.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0fda2ff0d084019ba4d7c6f371c95d8fd75ce3524c3cb8fb653a3023f6323e64" + +[[package]] +name = "simd-adler32" +version = "0.3.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d66dc143e6b11c1eddc06d5c423cfc97062865baf299914ab64caa38182078fe" + +[[package]] +name = "smallvec" +version = "1.13.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3c5e1a9a646d36c3599cd173a41282daf47c44583ad367b8e6837255952e5c67" + +[[package]] +name = "spade" +version = "2.12.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "93f5ef1f863aca7d1d7dda7ccfc36a0a4279bd6d3c375176e5e0712e25cb4889" +dependencies = [ + "hashbrown 0.14.5", + "num-traits", + "robust", + "smallvec", +] + +[[package]] +name = "stable_deref_trait" +version = "1.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a8f112729512f8e442d81f95a8a7ddf2b7c6b8a1a6f509a95864142b30cab2d3" + +[[package]] +name = "syn" +version = "2.0.90" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "919d3b74a5dd0ccd15aeb8f93e7006bd9e14c295087c9896a110f490752bcf31" +dependencies = [ + "proc-macro2", + "quote", + "unicode-ident", +] + +[[package]] +name = "target-lexicon" +version = "0.12.16" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "61c41af27dd6d1e27b1b16b489db798443478cef1f06a660c96db617ba5de3b1" + +[[package]] +name = "thiserror" +version = "1.0.69" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b6aaf5339b578ea85b50e080feb250a3e8ae8cfcdff9a461c9ec2904bc923f52" +dependencies = [ + "thiserror-impl", +] + +[[package]] +name = "thiserror-impl" +version = "1.0.69" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4fee6c4efc90059e10f81e6d42c60a18f76588c3d74cb83a0b242a2b6c7504c1" +dependencies = [ + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "ttf-parser" +version = "0.20.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "17f77d76d837a7830fe1d4f12b7b4ba4192c1888001c7164257e4bc6d21d96b4" + +[[package]] +name = "unicode-ident" +version = "1.0.14" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "adb9e6ca4f869e1180728b7950e35922a7fc6397f7b641499e8f3ef06e50dc83" + +[[package]] +name = "unindent" +version = "0.2.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c7de7d73e1754487cb58364ee906a499937a0dfabd86bcb980fa99ec8c8fa2ce" + +[[package]] +name = "version_check" +version = "0.9.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0b928f33d975fc6ad9f86c8f283853ad26bdd5b10b7f1542aa2fa15e2289105a" + +[[package]] +name = "walkdir" +version = "2.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "29790946404f91d9c5d06f9874efddea1dc06c5efe94541a7d6863108e3a5e4b" +dependencies = [ + "same-file", + "winapi-util", +] + +[[package]] +name = "wasi" +version = "0.11.0+wasi-snapshot-preview1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9c8d87e72b64a3b4db28d11ce29237c246188f4f51057d65a7eab63b7987e423" + +[[package]] +name = "wasm-bindgen" +version = "0.2.99" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a474f6281d1d70c17ae7aa6a613c87fce69a127e2624002df63dcb39d6cf6396" +dependencies = [ + "cfg-if", + "once_cell", + "wasm-bindgen-macro", +] + +[[package]] +name = "wasm-bindgen-backend" +version = "0.2.99" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5f89bb38646b4f81674e8f5c3fb81b562be1fd936d84320f3264486418519c79" +dependencies = [ + "bumpalo", + "log", + "proc-macro2", + "quote", + "syn", + "wasm-bindgen-shared", +] + +[[package]] +name = "wasm-bindgen-macro" +version = "0.2.99" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2cc6181fd9a7492eef6fef1f33961e3695e4579b9872a6f7c83aee556666d4fe" +dependencies = [ + "quote", + "wasm-bindgen-macro-support", +] + +[[package]] +name = "wasm-bindgen-macro-support" +version = "0.2.99" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "30d7a95b763d3c45903ed6c81f156801839e5ee968bb07e534c44df0fcd330c2" +dependencies = [ + "proc-macro2", + "quote", + "syn", + "wasm-bindgen-backend", + "wasm-bindgen-shared", +] + +[[package]] +name = "wasm-bindgen-shared" +version = "0.2.99" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "943aab3fdaaa029a6e0271b35ea10b72b943135afe9bffca82384098ad0e06a6" + +[[package]] +name = "web-sys" +version = "0.3.76" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "04dd7223427d52553d3702c004d3b2fe07c148165faa56313cb00211e31c12bc" +dependencies = [ + "js-sys", + "wasm-bindgen", +] + +[[package]] +name = "weezl" +version = "0.1.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "53a85b86a771b1c87058196170769dd264f66c0782acf1ae6cc51bfd64b39082" + +[[package]] +name = "winapi" +version = "0.3.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5c839a674fcd7a98952e593242ea400abe93992746761e38641405d28b00f419" +dependencies = [ + "winapi-i686-pc-windows-gnu", + "winapi-x86_64-pc-windows-gnu", +] + +[[package]] +name = "winapi-i686-pc-windows-gnu" +version = "0.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ac3b87c63620426dd9b991e5ce0329eff545bccbbb34f3be09ff6fb6ab51b7b6" + +[[package]] +name = "winapi-util" +version = "0.1.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cf221c93e13a30d793f7645a0e7762c55d169dbb0a49671918a2319d289b10bb" +dependencies = [ + "windows-sys 0.59.0", +] + +[[package]] +name = "winapi-x86_64-pc-windows-gnu" +version = "0.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "712e227841d057c1ee1cd2fb22fa7e5a5461ae8e48fa2ca79ec42cfc1931183f" + +[[package]] +name = "windows-core" +version = "0.52.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "33ab640c8d7e35bf8ba19b884ba838ceb4fba93a4e8c65a9059d08afcfc683d9" +dependencies = [ + "windows-targets 0.52.6", +] + +[[package]] +name = "windows-sys" +version = "0.48.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "677d2418bec65e3338edb076e806bc1ec15693c5d0104683f2efe857f61056a9" +dependencies = [ + "windows-targets 0.48.5", +] + +[[package]] +name = "windows-sys" +version = "0.59.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1e38bc4d79ed67fd075bcc251a1c39b32a1776bbe92e5bef1f0bf1f8c531853b" +dependencies = [ + "windows-targets 0.52.6", +] + +[[package]] +name = "windows-targets" +version = "0.48.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9a2fa6e2155d7247be68c096456083145c183cbbbc2764150dda45a87197940c" +dependencies = [ + "windows_aarch64_gnullvm 0.48.5", + "windows_aarch64_msvc 0.48.5", + "windows_i686_gnu 0.48.5", + "windows_i686_msvc 0.48.5", + "windows_x86_64_gnu 0.48.5", + "windows_x86_64_gnullvm 0.48.5", + "windows_x86_64_msvc 0.48.5", +] + +[[package]] +name = "windows-targets" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9b724f72796e036ab90c1021d4780d4d3d648aca59e491e6b98e725b84e99973" +dependencies = [ + "windows_aarch64_gnullvm 0.52.6", + "windows_aarch64_msvc 0.52.6", + "windows_i686_gnu 0.52.6", + "windows_i686_gnullvm", + "windows_i686_msvc 0.52.6", + "windows_x86_64_gnu 0.52.6", + "windows_x86_64_gnullvm 0.52.6", + "windows_x86_64_msvc 0.52.6", +] + +[[package]] +name = "windows_aarch64_gnullvm" +version = "0.48.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2b38e32f0abccf9987a4e3079dfb67dcd799fb61361e53e2882c3cbaf0d905d8" + +[[package]] +name = "windows_aarch64_gnullvm" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "32a4622180e7a0ec044bb555404c800bc9fd9ec262ec147edd5989ccd0c02cd3" + +[[package]] +name = "windows_aarch64_msvc" +version = "0.48.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dc35310971f3b2dbbf3f0690a219f40e2d9afcf64f9ab7cc1be722937c26b4bc" + +[[package]] +name = "windows_aarch64_msvc" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "09ec2a7bb152e2252b53fa7803150007879548bc709c039df7627cabbd05d469" + +[[package]] +name = "windows_i686_gnu" +version = "0.48.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a75915e7def60c94dcef72200b9a8e58e5091744960da64ec734a6c6e9b3743e" + +[[package]] +name = "windows_i686_gnu" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8e9b5ad5ab802e97eb8e295ac6720e509ee4c243f69d781394014ebfe8bbfa0b" + +[[package]] +name = "windows_i686_gnullvm" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0eee52d38c090b3caa76c563b86c3a4bd71ef1a819287c19d586d7334ae8ed66" + +[[package]] +name = "windows_i686_msvc" +version = "0.48.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8f55c233f70c4b27f66c523580f78f1004e8b5a8b659e05a4eb49d4166cca406" + +[[package]] +name = "windows_i686_msvc" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "240948bc05c5e7c6dabba28bf89d89ffce3e303022809e73deaefe4f6ec56c66" + +[[package]] +name = "windows_x86_64_gnu" +version = "0.48.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "53d40abd2583d23e4718fddf1ebec84dbff8381c07cae67ff7768bbf19c6718e" + +[[package]] +name = "windows_x86_64_gnu" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "147a5c80aabfbf0c7d901cb5895d1de30ef2907eb21fbbab29ca94c5b08b1a78" + +[[package]] +name = "windows_x86_64_gnullvm" +version = "0.48.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0b7b52767868a23d5bab768e390dc5f5c55825b6d30b86c844ff2dc7414044cc" + +[[package]] +name = "windows_x86_64_gnullvm" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "24d5b23dc417412679681396f2b49f3de8c1473deb516bd34410872eff51ed0d" + +[[package]] +name = "windows_x86_64_msvc" +version = "0.48.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ed94fce61571a4006852b7389a063ab983c02eb1bb37b47f8272ce92d06d9538" + +[[package]] +name = "windows_x86_64_msvc" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "589f6da84c646204747d1270a2a5661ea66ed1cced2631d546fdfb155959f9ec" + +[[package]] +name = "wio" +version = "0.2.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5d129932f4644ac2396cb456385cbf9e63b5b30c6e8dc4820bdca4eb082037a5" +dependencies = [ + "winapi", +] + +[[package]] +name = "yeslogic-fontconfig-sys" +version = "6.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "503a066b4c037c440169d995b869046827dbc71263f6e8f3be6d77d4f3229dbd" +dependencies = [ + "dlib", + "once_cell", + "pkg-config", +] + +[[package]] +name = "zerocopy" +version = "0.7.35" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1b9b4fd18abc82b8136838da5d50bae7bdea537c574d8dc1a34ed098d6c166f0" +dependencies = [ + "byteorder", + "zerocopy-derive", +] + +[[package]] +name = "zerocopy-derive" +version = "0.7.35" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "fa4f8080344d4671fb4e831a13ad1e68092748387dfc4f55e356242fae12ce3e" +dependencies = [ + "proc-macro2", + "quote", + "syn", +] diff --git a/Cargo.toml b/bitbots_navigation/bitbots_path_planning/Cargo.toml similarity index 100% rename from Cargo.toml rename to bitbots_navigation/bitbots_path_planning/Cargo.toml diff --git a/src/lib.rs b/bitbots_navigation/bitbots_path_planning/src/lib.rs similarity index 100% rename from src/lib.rs rename to bitbots_navigation/bitbots_path_planning/src/lib.rs diff --git a/src/map.rs b/bitbots_navigation/bitbots_path_planning/src/map.rs similarity index 100% rename from src/map.rs rename to bitbots_navigation/bitbots_path_planning/src/map.rs diff --git a/src/util.rs b/bitbots_navigation/bitbots_path_planning/src/util.rs similarity index 100% rename from src/util.rs rename to bitbots_navigation/bitbots_path_planning/src/util.rs diff --git a/test.py b/bitbots_navigation/bitbots_path_planning/test/test.py similarity index 100% rename from test.py rename to bitbots_navigation/bitbots_path_planning/test/test.py From 8e51c2b662f289f7b1404f992ec084f9c9d65f39 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Helena=20J=C3=A4ger?= Date: Wed, 18 Dec 2024 14:22:41 +0100 Subject: [PATCH 17/47] added README --- README.md | 1 + 1 file changed, 1 insertion(+) create mode 100644 README.md diff --git a/README.md b/README.md new file mode 100644 index 000000000..168c29bc4 --- /dev/null +++ b/README.md @@ -0,0 +1 @@ +# bitbots_pathplanning_rust \ No newline at end of file From 8c2b20be84a366000904c0f4d47578683b6b6e75 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Helena=20J=C3=A4ger?= Date: Wed, 18 Dec 2024 16:04:44 +0100 Subject: [PATCH 18/47] initial commit (untested) --- .github/workflows/CI.yml | 181 +++++++++++++ .gitignore | 72 ++++++ Cargo.lock | 545 +++++++++++++++++++++++++++++++++++++++ Cargo.toml | 15 ++ pyproject.toml | 15 ++ src/lib.rs | 18 ++ src/map.rs | 75 ++++++ src/obstacle.rs | 58 +++++ src/planner.rs | 119 +++++++++ 9 files changed, 1098 insertions(+) create mode 100644 .github/workflows/CI.yml create mode 100644 .gitignore create mode 100644 Cargo.lock create mode 100644 Cargo.toml create mode 100644 pyproject.toml create mode 100644 src/lib.rs create mode 100644 src/map.rs create mode 100644 src/obstacle.rs create mode 100644 src/planner.rs diff --git a/.github/workflows/CI.yml b/.github/workflows/CI.yml new file mode 100644 index 000000000..01e101321 --- /dev/null +++ b/.github/workflows/CI.yml @@ -0,0 +1,181 @@ +# This file is autogenerated by maturin v1.7.5 +# To update, run +# +# maturin generate-ci github +# +name: CI + +on: + push: + branches: + - main + - master + tags: + - '*' + pull_request: + workflow_dispatch: + +permissions: + contents: read + +jobs: + linux: + runs-on: ${{ matrix.platform.runner }} + strategy: + matrix: + platform: + - runner: ubuntu-22.04 + target: x86_64 + - runner: ubuntu-22.04 + target: x86 + - runner: ubuntu-22.04 + target: aarch64 + - runner: ubuntu-22.04 + target: armv7 + - runner: ubuntu-22.04 + target: s390x + - runner: ubuntu-22.04 + target: ppc64le + steps: + - uses: actions/checkout@v4 + - uses: actions/setup-python@v5 + with: + python-version: 3.x + - name: Build wheels + uses: PyO3/maturin-action@v1 + with: + target: ${{ matrix.platform.target }} + args: --release --out dist --find-interpreter + sccache: 'true' + manylinux: auto + - name: Upload wheels + uses: actions/upload-artifact@v4 + with: + name: wheels-linux-${{ matrix.platform.target }} + path: dist + + musllinux: + runs-on: ${{ matrix.platform.runner }} + strategy: + matrix: + platform: + - runner: ubuntu-22.04 + target: x86_64 + - runner: ubuntu-22.04 + target: x86 + - runner: ubuntu-22.04 + target: aarch64 + - runner: ubuntu-22.04 + target: armv7 + steps: + - uses: actions/checkout@v4 + - uses: actions/setup-python@v5 + with: + python-version: 3.x + - name: Build wheels + uses: PyO3/maturin-action@v1 + with: + target: ${{ matrix.platform.target }} + args: --release --out dist --find-interpreter + sccache: 'true' + manylinux: musllinux_1_2 + - name: Upload wheels + uses: actions/upload-artifact@v4 + with: + name: wheels-musllinux-${{ matrix.platform.target }} + path: dist + + windows: + runs-on: ${{ matrix.platform.runner }} + strategy: + matrix: + platform: + - runner: windows-latest + target: x64 + - runner: windows-latest + target: x86 + steps: + - uses: actions/checkout@v4 + - uses: actions/setup-python@v5 + with: + python-version: 3.x + architecture: ${{ matrix.platform.target }} + - name: Build wheels + uses: PyO3/maturin-action@v1 + with: + target: ${{ matrix.platform.target }} + args: --release --out dist --find-interpreter + sccache: 'true' + - name: Upload wheels + uses: actions/upload-artifact@v4 + with: + name: wheels-windows-${{ matrix.platform.target }} + path: dist + + macos: + runs-on: ${{ matrix.platform.runner }} + strategy: + matrix: + platform: + - runner: macos-13 + target: x86_64 + - runner: macos-14 + target: aarch64 + steps: + - uses: actions/checkout@v4 + - uses: actions/setup-python@v5 + with: + python-version: 3.x + - name: Build wheels + uses: PyO3/maturin-action@v1 + with: + target: ${{ matrix.platform.target }} + args: --release --out dist --find-interpreter + sccache: 'true' + - name: Upload wheels + uses: actions/upload-artifact@v4 + with: + name: wheels-macos-${{ matrix.platform.target }} + path: dist + + sdist: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + - name: Build sdist + uses: PyO3/maturin-action@v1 + with: + command: sdist + args: --out dist + - name: Upload sdist + uses: actions/upload-artifact@v4 + with: + name: wheels-sdist + path: dist + + release: + name: Release + runs-on: ubuntu-latest + if: ${{ startsWith(github.ref, 'refs/tags/') || github.event_name == 'workflow_dispatch' }} + needs: [linux, musllinux, windows, macos, sdist] + permissions: + # Use to sign the release artifacts + id-token: write + # Used to upload release artifacts + contents: write + # Used to generate artifact attestation + attestations: write + steps: + - uses: actions/download-artifact@v4 + - name: Generate artifact attestation + uses: actions/attest-build-provenance@v1 + with: + subject-path: 'wheels-*/*' + - name: Publish to PyPI + if: "startsWith(github.ref, 'refs/tags/')" + uses: PyO3/maturin-action@v1 + env: + MATURIN_PYPI_TOKEN: ${{ secrets.PYPI_API_TOKEN }} + with: + command: upload + args: --non-interactive --skip-existing wheels-*/* diff --git a/.gitignore b/.gitignore new file mode 100644 index 000000000..c8f044299 --- /dev/null +++ b/.gitignore @@ -0,0 +1,72 @@ +/target + +# Byte-compiled / optimized / DLL files +__pycache__/ +.pytest_cache/ +*.py[cod] + +# C extensions +*.so + +# Distribution / packaging +.Python +.venv/ +env/ +bin/ +build/ +develop-eggs/ +dist/ +eggs/ +lib/ +lib64/ +parts/ +sdist/ +var/ +include/ +man/ +venv/ +*.egg-info/ +.installed.cfg +*.egg + +# Installer logs +pip-log.txt +pip-delete-this-directory.txt +pip-selfcheck.json + +# Unit test / coverage reports +htmlcov/ +.tox/ +.coverage +.cache +nosetests.xml +coverage.xml + +# Translations +*.mo + +# Mr Developer +.mr.developer.cfg +.project +.pydevproject + +# Rope +.ropeproject + +# Django stuff: +*.log +*.pot + +.DS_Store + +# Sphinx documentation +docs/_build/ + +# PyCharm +.idea/ + +# VSCode +.vscode/ + +# Pyenv +.python-version diff --git a/Cargo.lock b/Cargo.lock new file mode 100644 index 000000000..dc316f713 --- /dev/null +++ b/Cargo.lock @@ -0,0 +1,545 @@ +# This file is automatically @generated by Cargo. +# It is not intended for manual editing. +version = 3 + +[[package]] +name = "ahash" +version = "0.8.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e89da841a80418a9b391ebaea17f5c112ffaaa96f621d2c285b5174da76b9011" +dependencies = [ + "cfg-if", + "once_cell", + "version_check", + "zerocopy", +] + +[[package]] +name = "allocator-api2" +version = "0.2.21" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "683d7910e743518b0e34f1186f92494becacb047c7b6bf616c96772180fef923" + +[[package]] +name = "approx" +version = "0.5.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cab112f0a86d568ea0e627cc1d6be74a1e9cd55214684db5561995f6dad897c6" +dependencies = [ + "num-traits", +] + +[[package]] +name = "autocfg" +version = "1.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ace50bade8e6234aa140d9a2f552bbee1db4d353f69b8217bc503490fc1a9f26" + +[[package]] +name = "bitbots_pathplanning_rust" +version = "0.1.0" +dependencies = [ + "geo", + "keyed_priority_queue", + "ordered-float", + "pyo3", +] + +[[package]] +name = "byteorder" +version = "1.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1fd0f2584146f6f2ef48085050886acf353beff7305ebd1ae69500e27c67f64b" + +[[package]] +name = "cfg-if" +version = "1.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "baf1de4339761588bc0619e3cbc0120ee582ebb74b53b4efbf79117bd2da40fd" + +[[package]] +name = "crossbeam-deque" +version = "0.8.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9dd111b7b7f7d55b72c0a6ae361660ee5853c9af73f70c3c2ef6858b950e2e51" +dependencies = [ + "crossbeam-epoch", + "crossbeam-utils", +] + +[[package]] +name = "crossbeam-epoch" +version = "0.9.18" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5b82ac4a3c2ca9c3460964f020e1402edd5753411d7737aa39c3714ad1b5420e" +dependencies = [ + "crossbeam-utils", +] + +[[package]] +name = "crossbeam-utils" +version = "0.8.21" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d0a5c400df2834b80a4c3327b3aad3a4c4cd4de0629063962b03235697506a28" + +[[package]] +name = "earcutr" +version = "0.4.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "79127ed59a85d7687c409e9978547cffb7dc79675355ed22da6b66fd5f6ead01" +dependencies = [ + "itertools", + "num-traits", +] + +[[package]] +name = "either" +version = "1.13.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "60b1af1c220855b6ceac025d3f6ecdd2b7c4894bfe9cd9bda4fbb4bc7c0d4cf0" + +[[package]] +name = "equivalent" +version = "1.0.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5443807d6dff69373d433ab9ef5378ad8df50ca6298caf15de6e52e24aaf54d5" + +[[package]] +name = "float_next_after" +version = "1.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8bf7cc16383c4b8d58b9905a8509f02926ce3058053c056376248d958c9df1e8" + +[[package]] +name = "geo" +version = "0.29.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "34f0e6e028c581e82e6822a68869514e94c25e7f8ea669a2d8595bdf7461ccc5" +dependencies = [ + "earcutr", + "float_next_after", + "geo-types", + "geographiclib-rs", + "i_overlay", + "log", + "num-traits", + "robust", + "rstar", + "spade", +] + +[[package]] +name = "geo-types" +version = "0.7.14" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b6f47c611187777bbca61ea7aba780213f5f3441fd36294ab333e96cfa791b65" +dependencies = [ + "approx", + "num-traits", + "rayon", + "rstar", + "serde", +] + +[[package]] +name = "geographiclib-rs" +version = "0.2.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e6e5ed84f8089c70234b0a8e0aedb6dc733671612ddc0d37c6066052f9781960" +dependencies = [ + "libm", +] + +[[package]] +name = "hash32" +version = "0.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "47d60b12902ba28e2730cd37e95b8c9223af2808df9e902d4df49588d1470606" +dependencies = [ + "byteorder", +] + +[[package]] +name = "hashbrown" +version = "0.14.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e5274423e17b7c9fc20b6e7e208532f9b19825d82dfd615708b70edd83df41f1" +dependencies = [ + "ahash", + "allocator-api2", +] + +[[package]] +name = "hashbrown" +version = "0.15.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bf151400ff0baff5465007dd2f3e717f3fe502074ca563069ce3a6629d07b289" + +[[package]] +name = "heapless" +version = "0.8.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0bfb9eb618601c89945a70e254898da93b13be0388091d42117462b265bb3fad" +dependencies = [ + "hash32", + "stable_deref_trait", +] + +[[package]] +name = "heck" +version = "0.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2304e00983f87ffb38b55b444b5e3b60a884b5d30c0fca7d82fe33449bbe55ea" + +[[package]] +name = "i_float" +version = "1.6.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "775f9961a8d2f879725da8aff789bb20a3ddf297473e0c90af75e69313919490" +dependencies = [ + "serde", +] + +[[package]] +name = "i_key_sort" +version = "0.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "347c253b4748a1a28baf94c9ce133b6b166f08573157e05afe718812bc599fcd" + +[[package]] +name = "i_overlay" +version = "1.9.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "06740cd31c1f963823e007d8e6edcd2db634b2856f4f613e3df01737fd852482" +dependencies = [ + "i_float", + "i_key_sort", + "i_shape", + "i_tree", + "rayon", +] + +[[package]] +name = "i_shape" +version = "1.6.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "27dbe9e5238d6b9c694c08415bf00fb370b089949bd818ab01f41f8927b8774c" +dependencies = [ + "i_float", + "serde", +] + +[[package]] +name = "i_tree" +version = "0.8.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "155181bc97d770181cf9477da51218a19ee92a8e5be642e796661aee2b601139" + +[[package]] +name = "indexmap" +version = "2.7.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "62f822373a4fe84d4bb149bf54e584a7f4abec90e072ed49cda0edea5b95471f" +dependencies = [ + "equivalent", + "hashbrown 0.15.2", +] + +[[package]] +name = "indoc" +version = "2.0.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b248f5224d1d606005e02c97f5aa4e88eeb230488bcc03bc9ca4d7991399f2b5" + +[[package]] +name = "itertools" +version = "0.11.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b1c173a5686ce8bfa551b3563d0c2170bf24ca44da99c7ca4bfdab5418c3fe57" +dependencies = [ + "either", +] + +[[package]] +name = "keyed_priority_queue" +version = "0.4.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4ee7893dab2e44ae5f9d0173f26ff4aa327c10b01b06a72b52dd9405b628640d" +dependencies = [ + "indexmap", +] + +[[package]] +name = "libc" +version = "0.2.168" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5aaeb2981e0606ca11d79718f8bb01164f1d6ed75080182d3abf017e6d244b6d" + +[[package]] +name = "libm" +version = "0.2.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8355be11b20d696c8f18f6cc018c4e372165b1fa8126cef092399c9951984ffa" + +[[package]] +name = "log" +version = "0.4.22" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a7a70ba024b9dc04c27ea2f0c0548feb474ec5c54bba33a7f72f873a39d07b24" + +[[package]] +name = "memoffset" +version = "0.9.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "488016bfae457b036d996092f6cb448677611ce4449e970ceaf42695203f218a" +dependencies = [ + "autocfg", +] + +[[package]] +name = "num-traits" +version = "0.2.19" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "071dfc062690e90b734c0b2273ce72ad0ffa95f0c74596bc250dcfd960262841" +dependencies = [ + "autocfg", + "libm", +] + +[[package]] +name = "once_cell" +version = "1.20.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1261fe7e33c73b354eab43b1273a57c8f967d0391e80353e51f764ac02cf6775" + +[[package]] +name = "ordered-float" +version = "4.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c65ee1f9701bf938026630b455d5315f490640234259037edb259798b3bcf85e" +dependencies = [ + "num-traits", +] + +[[package]] +name = "portable-atomic" +version = "1.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "280dc24453071f1b63954171985a0b0d30058d287960968b9b2aca264c8d4ee6" + +[[package]] +name = "proc-macro2" +version = "1.0.92" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "37d3544b3f2748c54e147655edb5025752e2303145b5aefb3c3ea2c78b973bb0" +dependencies = [ + "unicode-ident", +] + +[[package]] +name = "pyo3" +version = "0.23.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e484fd2c8b4cb67ab05a318f1fd6fa8f199fcc30819f08f07d200809dba26c15" +dependencies = [ + "cfg-if", + "indoc", + "libc", + "memoffset", + "once_cell", + "portable-atomic", + "pyo3-build-config", + "pyo3-ffi", + "pyo3-macros", + "unindent", +] + +[[package]] +name = "pyo3-build-config" +version = "0.23.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dc0e0469a84f208e20044b98965e1561028180219e35352a2afaf2b942beff3b" +dependencies = [ + "once_cell", + "target-lexicon", +] + +[[package]] +name = "pyo3-ffi" +version = "0.23.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "eb1547a7f9966f6f1a0f0227564a9945fe36b90da5a93b3933fc3dc03fae372d" +dependencies = [ + "libc", + "pyo3-build-config", +] + +[[package]] +name = "pyo3-macros" +version = "0.23.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "fdb6da8ec6fa5cedd1626c886fc8749bdcbb09424a86461eb8cdf096b7c33257" +dependencies = [ + "proc-macro2", + "pyo3-macros-backend", + "quote", + "syn", +] + +[[package]] +name = "pyo3-macros-backend" +version = "0.23.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "38a385202ff5a92791168b1136afae5059d3ac118457bb7bc304c197c2d33e7d" +dependencies = [ + "heck", + "proc-macro2", + "pyo3-build-config", + "quote", + "syn", +] + +[[package]] +name = "quote" +version = "1.0.37" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b5b9d34b8991d19d98081b46eacdd8eb58c6f2b201139f7c5f643cc155a633af" +dependencies = [ + "proc-macro2", +] + +[[package]] +name = "rayon" +version = "1.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b418a60154510ca1a002a752ca9714984e21e4241e804d32555251faf8b78ffa" +dependencies = [ + "either", + "rayon-core", +] + +[[package]] +name = "rayon-core" +version = "1.12.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1465873a3dfdaa8ae7cb14b4383657caab0b3e8a0aa9ae8e04b044854c8dfce2" +dependencies = [ + "crossbeam-deque", + "crossbeam-utils", +] + +[[package]] +name = "robust" +version = "1.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cbf4a6aa5f6d6888f39e980649f3ad6b666acdce1d78e95b8a2cb076e687ae30" + +[[package]] +name = "rstar" +version = "0.12.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "421400d13ccfd26dfa5858199c30a5d76f9c54e0dba7575273025b43c5175dbb" +dependencies = [ + "heapless", + "num-traits", + "smallvec", +] + +[[package]] +name = "serde" +version = "1.0.216" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0b9781016e935a97e8beecf0c933758c97a5520d32930e460142b4cd80c6338e" +dependencies = [ + "serde_derive", +] + +[[package]] +name = "serde_derive" +version = "1.0.216" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "46f859dbbf73865c6627ed570e78961cd3ac92407a2d117204c49232485da55e" +dependencies = [ + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "smallvec" +version = "1.13.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3c5e1a9a646d36c3599cd173a41282daf47c44583ad367b8e6837255952e5c67" + +[[package]] +name = "spade" +version = "2.12.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "93f5ef1f863aca7d1d7dda7ccfc36a0a4279bd6d3c375176e5e0712e25cb4889" +dependencies = [ + "hashbrown 0.14.5", + "num-traits", + "robust", + "smallvec", +] + +[[package]] +name = "stable_deref_trait" +version = "1.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a8f112729512f8e442d81f95a8a7ddf2b7c6b8a1a6f509a95864142b30cab2d3" + +[[package]] +name = "syn" +version = "2.0.90" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "919d3b74a5dd0ccd15aeb8f93e7006bd9e14c295087c9896a110f490752bcf31" +dependencies = [ + "proc-macro2", + "quote", + "unicode-ident", +] + +[[package]] +name = "target-lexicon" +version = "0.12.16" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "61c41af27dd6d1e27b1b16b489db798443478cef1f06a660c96db617ba5de3b1" + +[[package]] +name = "unicode-ident" +version = "1.0.14" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "adb9e6ca4f869e1180728b7950e35922a7fc6397f7b641499e8f3ef06e50dc83" + +[[package]] +name = "unindent" +version = "0.2.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c7de7d73e1754487cb58364ee906a499937a0dfabd86bcb980fa99ec8c8fa2ce" + +[[package]] +name = "version_check" +version = "0.9.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0b928f33d975fc6ad9f86c8f283853ad26bdd5b10b7f1542aa2fa15e2289105a" + +[[package]] +name = "zerocopy" +version = "0.7.35" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1b9b4fd18abc82b8136838da5d50bae7bdea537c574d8dc1a34ed098d6c166f0" +dependencies = [ + "zerocopy-derive", +] + +[[package]] +name = "zerocopy-derive" +version = "0.7.35" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "fa4f8080344d4671fb4e831a13ad1e68092748387dfc4f55e356242fae12ce3e" +dependencies = [ + "proc-macro2", + "quote", + "syn", +] diff --git a/Cargo.toml b/Cargo.toml new file mode 100644 index 000000000..39c62ef28 --- /dev/null +++ b/Cargo.toml @@ -0,0 +1,15 @@ +[package] +name = "bitbots_pathplanning_rust" +version = "0.1.0" +edition = "2021" + +# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html +[lib] +name = "bitbots_pathplanning_rust" +crate-type = ["cdylib"] + +[dependencies] +geo = "0.29.3" +keyed_priority_queue = "0.4.2" +ordered-float = "4.5.0" +pyo3 = "0.23.1" diff --git a/pyproject.toml b/pyproject.toml new file mode 100644 index 000000000..14318d892 --- /dev/null +++ b/pyproject.toml @@ -0,0 +1,15 @@ +[build-system] +requires = ["maturin>=1.7,<2.0"] +build-backend = "maturin" + +[project] +name = "bitbots_pathplanning_rust" +requires-python = ">=3.8" +classifiers = [ + "Programming Language :: Rust", + "Programming Language :: Python :: Implementation :: CPython", + "Programming Language :: Python :: Implementation :: PyPy", +] +dynamic = ["version"] +[tool.maturin] +features = ["pyo3/extension-module"] diff --git a/src/lib.rs b/src/lib.rs new file mode 100644 index 000000000..665e69766 --- /dev/null +++ b/src/lib.rs @@ -0,0 +1,18 @@ +use pyo3::prelude::*; + +mod map; +mod obstacle; +mod planner; + +/// Formats the sum of two numbers as string. +#[pyfunction] +fn sum_as_string(a: usize, b: usize) -> PyResult { + Ok((a + b).to_string()) +} + +/// A Python module implemented in Rust. +#[pymodule] +fn bitbots_pathplanning_rust(m: &Bound<'_, PyModule>) -> PyResult<()> { + m.add_function(wrap_pyfunction!(sum_as_string, m)?)?; + Ok(()) +} diff --git a/src/map.rs b/src/map.rs new file mode 100644 index 000000000..f4fe57b53 --- /dev/null +++ b/src/map.rs @@ -0,0 +1,75 @@ +use geo::{BooleanOps, Coord, LineString, MultiPolygon, Polygon}; +use pyo3::prelude::*; + +use crate::{obstacle::{Obstacle, RoundObstacle}, planner::PathPlanner}; + +/// Configuration values for the ObstacleMap, these should be given by ROS parameters +#[pyclass( + eq, + str = "ObstacleMapConfig(dilate={dilate:?} num_vertices={num_vertices:?})" +)] +#[derive(Debug, Copy, Clone, PartialEq)] +pub struct ObstacleMapConfig { + /// A distance by which the obstacles should be dilated + #[pyo3(set, get)] + pub dilate: f64, + /// The number of vertices that the polygon representing an obstacle has + #[pyo3(set, get)] + pub num_vertices: usize, +} + +#[pymethods] +impl ObstacleMapConfig { + #[new] + /// Create a new config with the given values + pub fn new(dilate: f64, num_vertices: usize) -> Self { + Self { + dilate, + num_vertices, + } + } +} + +#[pyclass(eq, str = "ObstacleMap(obstacles={obstacles:?})")] +#[derive(Debug, Clone, PartialEq)] +pub struct ObstacleMap { + config: ObstacleMapConfig, + obstacles: Vec, +} + +#[pymethods] +impl ObstacleMap { + #[new] + pub fn new(config: ObstacleMapConfig, obstacles: Vec) -> Self { + Self { config, obstacles } + } + + pub fn shortest_path(&self, start: (f64, f64), goal: (f64, f64)) -> Vec<(f64, f64)> { + PathPlanner::new(&self, start, goal).shortest_path() + } +} + +impl ObstacleMap { + pub fn as_vertices(&self) -> Vec { + self.obstacles + .iter() + .map(|obstacle| obstacle.as_vertices(&self.config)) + .flatten() + .collect() + } + + fn as_polygons(&self) -> Vec { + self.obstacles + .iter() + .map(|obstacle| obstacle.as_polygon(&self.config)) + .collect() + } + + pub fn as_multipolygon(&self) -> MultiPolygon { + let polygons = self.as_polygons(); + polygons.iter().fold( + MultiPolygon::new(vec![Polygon::new(LineString::new(vec![]), vec![])]), + |a, b| a.union(b), + ) + } +} diff --git a/src/obstacle.rs b/src/obstacle.rs new file mode 100644 index 000000000..240e5d4e6 --- /dev/null +++ b/src/obstacle.rs @@ -0,0 +1,58 @@ +use pyo3::prelude::*; + +use geo::{Coord, LineString, Polygon}; + +use crate::map::ObstacleMapConfig; + +pub trait Obstacle { + /// Return the vertices of this obstacle for the visibility graph + fn as_vertices(&self, config: &ObstacleMapConfig) -> Vec; + + /// Return the polygon representation of this obstacle for intersection checking + fn as_polygon(&self, config: &ObstacleMapConfig) -> Polygon; +} + +/// A round obstacle +#[pyclass(eq, str = "RoundObstacle(center={center:?} radius={radius:?})")] +#[derive(Debug, Clone, Copy, PartialEq)] +pub struct RoundObstacle { + /// The center of the obstacle + center: (f64, f64), + /// The radius of the obstacle + radius: f64, +} + +#[pymethods] +impl RoundObstacle { + #[new] + pub fn new(center: (f64, f64), radius: f64) -> Self { + Self { center, radius } + } +} + +impl RoundObstacle { + /// A helper function to generate a num_vertices-sided Polygon with own radius plus offset + fn regular_ngon(&self, num_vertices: usize, offset: f64) -> Vec { + (0..num_vertices) + .map(|side| { + let angle = (side as f64) / (num_vertices as f64) * std::f64::consts::TAU; + let radius = self.radius + offset; + Coord::from(self.center) + Coord::from((radius * angle.cos(), radius * angle.sin())) + }) + .collect::>() + } +} + +/// Value subtracted from the radius of obstacles for intersection checking +pub const EPSILON: f64 = 0.01; + +impl Obstacle for RoundObstacle { + fn as_vertices(&self, config: &ObstacleMapConfig) -> Vec { + self.regular_ngon(config.num_vertices, config.dilate) + } + + fn as_polygon(&self, config: &ObstacleMapConfig) -> Polygon { + let vertices = self.regular_ngon(config.num_vertices, config.dilate - EPSILON); + Polygon::new(LineString::new(vertices), vec![]) + } +} diff --git a/src/planner.rs b/src/planner.rs new file mode 100644 index 000000000..2e516380b --- /dev/null +++ b/src/planner.rs @@ -0,0 +1,119 @@ +use std::{cmp::Reverse, collections::{HashMap, HashSet}}; + +use geo::{Coord, Distance, Euclidean, Intersects, Line, MultiPolygon}; +use keyed_priority_queue::{Entry, KeyedPriorityQueue}; +use ordered_float::OrderedFloat; + +use crate::{map::ObstacleMap, obstacle::EPSILON}; + +#[derive(Debug, Clone)] +pub struct PathPlanner { + vertices: Vec, + obstacle: MultiPolygon, + start: usize, + goal: usize, + open: KeyedPriorityQueue>>, + g_score: HashMap>, + successors: HashSet, + from: HashMap, +} + +impl PathPlanner { + pub fn new(map: &ObstacleMap, start: (f64, f64), goal: (f64, f64)) -> Self { + let mut vertices = map.as_vertices(); + vertices.append(&mut vec![start.into(), goal.into()]); + let obstacle = map.as_multipolygon(); + let start = vertices.len() - 2; + let goal = vertices.len() - 1; + let mut open = KeyedPriorityQueue::>>::new(); + open.push( + start, + Reverse(OrderedFloat(Euclidean::distance( + vertices[start], + vertices[goal], + ))), + ); + let from = HashMap::::new(); + let successors = HashSet::::from_iter(0..vertices.len()); + let mut g_score = HashMap::>::new(); + g_score.insert(start, OrderedFloat(0.0)); + Self { + vertices, + obstacle, + start, + goal, + open, + g_score, + successors, + from, + } + } + + fn heuristic(&self, node: usize) -> OrderedFloat { + self.distance(node, self.goal) + } + + fn distance(&self, from: usize, to: usize) -> OrderedFloat { + OrderedFloat(Euclidean::distance(self.vertices[from], self.vertices[to])) + } + + fn reconstruct_path(&self, vertex: usize) -> Vec<(f64, f64)> { + let mut path = vec![vertex]; + let mut current = vertex; + while let Some(previous) = self.from.get(¤t) { + current = *previous; + path.push(*previous); + } + path.reverse(); + return path + .into_iter() + .map(|idx| self.vertices[idx].x_y()) + .collect(); + } + + fn expand_node(&mut self, vertex: usize) { + let g_vertex = self + .g_score + .get(&vertex) + .unwrap_or(&OrderedFloat(std::f64::INFINITY)) + .clone(); + for successor in self.successors.iter().cloned() { + if self.obstacle.intersects(&Line::new(self.vertices[vertex], self.vertices[successor])) { + continue; + } + let g_successor = self.g_score.get(&successor).unwrap_or(&OrderedFloat(std::f64::INFINITY)).clone(); + let g_tentative = g_vertex + self.distance(vertex, successor); + if g_tentative < g_successor { + self.from.insert(successor, vertex); + self.g_score.insert(successor, g_tentative); + let new_f_score = Reverse(g_tentative + self.heuristic(successor)); + match self.open.entry(successor) { + Entry::Occupied(entry) => { + entry.set_priority(new_f_score); + }, + Entry::Vacant(entry) => { + entry.set_priority(new_f_score); + }, + } + } + } + } + + pub fn shortest_path(mut self) -> Vec<(f64, f64)> { + let mut closest_vertex = self.start; + let mut closest_distance = OrderedFloat(std::f64::INFINITY); + while let Some((vertex, _)) = self.open.pop() { + if self.heuristic(vertex) < closest_distance - EPSILON { + closest_vertex = vertex; + closest_distance = self.heuristic(vertex); + } + if vertex == self.goal { + return self.reconstruct_path(vertex); + } + self.successors.remove(&vertex); + self.expand_node(vertex); + } + self.from.insert(self.goal, closest_vertex); + self.reconstruct_path(self.goal) + } +} From 5246d3603cd95f7d9b9443a5e6b5a045c5298d92 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Helena=20J=C3=A4ger?= Date: Wed, 18 Dec 2024 16:18:03 +0100 Subject: [PATCH 19/47] fixed module initialization --- src/lib.rs | 13 +++++-------- 1 file changed, 5 insertions(+), 8 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 665e69766..0cddab3de 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -1,18 +1,15 @@ +use map::{ObstacleMap, ObstacleMapConfig}; +use obstacle::RoundObstacle; use pyo3::prelude::*; mod map; mod obstacle; mod planner; -/// Formats the sum of two numbers as string. -#[pyfunction] -fn sum_as_string(a: usize, b: usize) -> PyResult { - Ok((a + b).to_string()) -} - -/// A Python module implemented in Rust. #[pymodule] fn bitbots_pathplanning_rust(m: &Bound<'_, PyModule>) -> PyResult<()> { - m.add_function(wrap_pyfunction!(sum_as_string, m)?)?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; Ok(()) } From 1b04e65862cf0b12cb5d66ec46357cf38d914a7a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Helena=20J=C3=A4ger?= Date: Wed, 18 Dec 2024 16:23:13 +0100 Subject: [PATCH 20/47] made fields readable --- src/map.rs | 2 ++ src/obstacle.rs | 2 ++ 2 files changed, 4 insertions(+) diff --git a/src/map.rs b/src/map.rs index f4fe57b53..66306a425 100644 --- a/src/map.rs +++ b/src/map.rs @@ -33,7 +33,9 @@ impl ObstacleMapConfig { #[pyclass(eq, str = "ObstacleMap(obstacles={obstacles:?})")] #[derive(Debug, Clone, PartialEq)] pub struct ObstacleMap { + #[pyo3(get, set)] config: ObstacleMapConfig, + #[pyo3(get, set)] obstacles: Vec, } diff --git a/src/obstacle.rs b/src/obstacle.rs index 240e5d4e6..1f50408d9 100644 --- a/src/obstacle.rs +++ b/src/obstacle.rs @@ -17,8 +17,10 @@ pub trait Obstacle { #[derive(Debug, Clone, Copy, PartialEq)] pub struct RoundObstacle { /// The center of the obstacle + #[pyo3(get, set)] center: (f64, f64), /// The radius of the obstacle + #[pyo3(get, set)] radius: f64, } From 26b99cc8a41cb163f1a8277a9073f80b996a6b23 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Helena=20J=C3=A4ger?= Date: Wed, 18 Dec 2024 16:39:41 +0100 Subject: [PATCH 21/47] changed README --- README.md | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 168c29bc4..ccbf0abb8 100644 --- a/README.md +++ b/README.md @@ -1 +1,17 @@ -# bitbots_pathplanning_rust \ No newline at end of file +# bitbots_pathplanning_rust + +A best-effort A*-on-visibility-graph implementation for pathplanning in obstacles maps where obstacles and the robot itself are assumed to be round. + +It exports three classes + +## `RoundObstacle` + +Represents a round obstacle with public fields `center: (float, float)` and `radius: float`. Can be created with the constructor `RoundObstacle(center, radius)` + +## `ObstacleMapConfig` + +Represents all configuration values for how obstacles should be treated with public fields `dilate: float` - the value by which the radii of the obstacles should be dilated (this should be set to approximately your own radius) - and `num_vertices` - the number of vertices the polygons approximating the round obstacles should have. Can be created with the constructor `ObstacleMapConfig(dilate, num_vertices)` + +## `ObstacleMap` + +Represents a set of obstacles on a plane with a given config. This has two fields: `config: ObstacleMapConfig` and `obstacles: [RoundObstacle]`. Can be created with the constructor `ObstacleMap(config, obstacles)` \ No newline at end of file From ce5dd61d51e923bfc30b9a0da4b9a245925e9826 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Helena=20J=C3=A4ger?= Date: Wed, 18 Dec 2024 18:08:24 +0100 Subject: [PATCH 22/47] integrated external rust pathplanning --- .../bitbots_path_planning/Cargo.lock | 1444 ----------------- .../bitbots_path_planning/Cargo.toml | 20 - .../bitbots_path_planning/map.py | 118 -- .../bitbots_path_planning/path_planning.py | 17 +- .../bitbots_path_planning/planner.py | 204 +-- .../bitbots_path_planning/src/lib.rs | 13 - .../bitbots_path_planning/src/map.rs | 245 --- .../bitbots_path_planning/src/util.rs | 11 - requirements/common.txt | 3 +- 9 files changed, 71 insertions(+), 2004 deletions(-) delete mode 100644 bitbots_navigation/bitbots_path_planning/Cargo.lock delete mode 100644 bitbots_navigation/bitbots_path_planning/Cargo.toml delete mode 100644 bitbots_navigation/bitbots_path_planning/bitbots_path_planning/map.py delete mode 100644 bitbots_navigation/bitbots_path_planning/src/lib.rs delete mode 100644 bitbots_navigation/bitbots_path_planning/src/map.rs delete mode 100644 bitbots_navigation/bitbots_path_planning/src/util.rs diff --git a/bitbots_navigation/bitbots_path_planning/Cargo.lock b/bitbots_navigation/bitbots_path_planning/Cargo.lock deleted file mode 100644 index 2d3bfec03..000000000 --- a/bitbots_navigation/bitbots_path_planning/Cargo.lock +++ /dev/null @@ -1,1444 +0,0 @@ -# This file is automatically @generated by Cargo. -# It is not intended for manual editing. -version = 3 - -[[package]] -name = "adler2" -version = "2.0.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "512761e0bb2578dd7380c6baaa0f4ce03e84f95e960231d1dec8bf4d7d6e2627" - -[[package]] -name = "ahash" -version = "0.8.11" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e89da841a80418a9b391ebaea17f5c112ffaaa96f621d2c285b5174da76b9011" -dependencies = [ - "cfg-if", - "once_cell", - "version_check", - "zerocopy", -] - -[[package]] -name = "allocator-api2" -version = "0.2.21" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "683d7910e743518b0e34f1186f92494becacb047c7b6bf616c96772180fef923" - -[[package]] -name = "android-tzdata" -version = "0.1.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e999941b234f3131b00bc13c22d06e8c5ff726d1b6318ac7eb276997bbb4fef0" - -[[package]] -name = "android_system_properties" -version = "0.1.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "819e7219dbd41043ac279b19830f2efc897156490d7fd6ea916720117ee66311" -dependencies = [ - "libc", -] - -[[package]] -name = "anyhow" -version = "1.0.94" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c1fd03a028ef38ba2276dce7e33fcd6369c158a1bca17946c4b1b701891c1ff7" - -[[package]] -name = "approx" -version = "0.5.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "cab112f0a86d568ea0e627cc1d6be74a1e9cd55214684db5561995f6dad897c6" -dependencies = [ - "num-traits", -] - -[[package]] -name = "autocfg" -version = "1.4.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ace50bade8e6234aa140d9a2f552bbee1db4d353f69b8217bc503490fc1a9f26" - -[[package]] -name = "bbpprs" -version = "0.1.0" -dependencies = [ - "anyhow", - "geo", - "itertools 0.13.0", - "keyed_priority_queue", - "ordered-float", - "petgraph", - "plotters", - "pyo3", - "rand", -] - -[[package]] -name = "bitflags" -version = "1.3.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "bef38d45163c2f1dde094a7dfd33ccf595c92905c8f8f4fdc18d06fb1037718a" - -[[package]] -name = "bitflags" -version = "2.6.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b048fb63fd8b5923fc5aa7b340d8e156aec7ec02f0c78fa8a6ddc2613f6f71de" - -[[package]] -name = "bumpalo" -version = "3.16.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "79296716171880943b8470b5f8d03aa55eb2e645a4874bdbb28adb49162e012c" - -[[package]] -name = "bytemuck" -version = "1.20.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8b37c88a63ffd85d15b406896cc343916d7cf57838a847b3a6f2ca5d39a5695a" - -[[package]] -name = "byteorder" -version = "1.5.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "1fd0f2584146f6f2ef48085050886acf353beff7305ebd1ae69500e27c67f64b" - -[[package]] -name = "cc" -version = "1.2.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "27f657647bcff5394bf56c7317665bbf790a137a50eaaa5c6bfbb9e27a518f2d" -dependencies = [ - "shlex", -] - -[[package]] -name = "cfg-if" -version = "1.0.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "baf1de4339761588bc0619e3cbc0120ee582ebb74b53b4efbf79117bd2da40fd" - -[[package]] -name = "chrono" -version = "0.4.39" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7e36cc9d416881d2e24f9a963be5fb1cd90966419ac844274161d10488b3e825" -dependencies = [ - "android-tzdata", - "iana-time-zone", - "js-sys", - "num-traits", - "wasm-bindgen", - "windows-targets 0.52.6", -] - -[[package]] -name = "color_quant" -version = "1.1.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3d7b894f5411737b7867f4827955924d7c254fc9f4d91a6aad6b097804b1018b" - -[[package]] -name = "core-foundation" -version = "0.9.4" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "91e195e091a93c46f7102ec7818a2aa394e1e1771c3ab4825963fa03e45afb8f" -dependencies = [ - "core-foundation-sys", - "libc", -] - -[[package]] -name = "core-foundation-sys" -version = "0.8.7" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "773648b94d0e5d620f64f280777445740e61fe701025087ec8b57f45c791888b" - -[[package]] -name = "core-graphics" -version = "0.23.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c07782be35f9e1140080c6b96f0d44b739e2278479f64e02fdab4e32dfd8b081" -dependencies = [ - "bitflags 1.3.2", - "core-foundation", - "core-graphics-types", - "foreign-types", - "libc", -] - -[[package]] -name = "core-graphics-types" -version = "0.1.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "45390e6114f68f718cc7a830514a96f903cccd70d02a8f6d9f643ac4ba45afaf" -dependencies = [ - "bitflags 1.3.2", - "core-foundation", - "libc", -] - -[[package]] -name = "core-text" -version = "20.1.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c9d2790b5c08465d49f8dc05c8bcae9fea467855947db39b0f8145c091aaced5" -dependencies = [ - "core-foundation", - "core-graphics", - "foreign-types", - "libc", -] - -[[package]] -name = "crc32fast" -version = "1.4.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a97769d94ddab943e4510d138150169a2758b5ef3eb191a9ee688de3e23ef7b3" -dependencies = [ - "cfg-if", -] - -[[package]] -name = "crossbeam-deque" -version = "0.8.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "613f8cc01fe9cf1a3eb3d7f488fd2fa8388403e97039e2f73692932e291a770d" -dependencies = [ - "crossbeam-epoch", - "crossbeam-utils", -] - -[[package]] -name = "crossbeam-epoch" -version = "0.9.18" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5b82ac4a3c2ca9c3460964f020e1402edd5753411d7737aa39c3714ad1b5420e" -dependencies = [ - "crossbeam-utils", -] - -[[package]] -name = "crossbeam-utils" -version = "0.8.20" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "22ec99545bb0ed0ea7bb9b8e1e9122ea386ff8a48c0922e43f36d45ab09e0e80" - -[[package]] -name = "dirs" -version = "5.0.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "44c45a9d03d6676652bcb5e724c7e988de1acad23a711b5217ab9cbecbec2225" -dependencies = [ - "dirs-sys", -] - -[[package]] -name = "dirs-sys" -version = "0.4.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "520f05a5cbd335fae5a99ff7a6ab8627577660ee5cfd6a94a6a929b52ff0321c" -dependencies = [ - "libc", - "option-ext", - "redox_users", - "windows-sys 0.48.0", -] - -[[package]] -name = "dlib" -version = "0.5.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "330c60081dcc4c72131f8eb70510f1ac07223e5d4163db481a04a0befcffa412" -dependencies = [ - "libloading", -] - -[[package]] -name = "dwrote" -version = "0.11.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "70182709525a3632b2ba96b6569225467b18ecb4a77f46d255f713a6bebf05fd" -dependencies = [ - "lazy_static", - "libc", - "winapi", - "wio", -] - -[[package]] -name = "earcutr" -version = "0.4.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "79127ed59a85d7687c409e9978547cffb7dc79675355ed22da6b66fd5f6ead01" -dependencies = [ - "itertools 0.11.0", - "num-traits", -] - -[[package]] -name = "either" -version = "1.13.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "60b1af1c220855b6ceac025d3f6ecdd2b7c4894bfe9cd9bda4fbb4bc7c0d4cf0" - -[[package]] -name = "equivalent" -version = "1.0.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5443807d6dff69373d433ab9ef5378ad8df50ca6298caf15de6e52e24aaf54d5" - -[[package]] -name = "fdeflate" -version = "0.3.7" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "1e6853b52649d4ac5c0bd02320cddc5ba956bdb407c4b75a2c6b75bf51500f8c" -dependencies = [ - "simd-adler32", -] - -[[package]] -name = "fixedbitset" -version = "0.4.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0ce7134b9999ecaf8bcd65542e436736ef32ddca1b3e06094cb6ec5755203b80" - -[[package]] -name = "flate2" -version = "1.0.35" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c936bfdafb507ebbf50b8074c54fa31c5be9a1e7e5f467dd659697041407d07c" -dependencies = [ - "crc32fast", - "miniz_oxide", -] - -[[package]] -name = "float-ord" -version = "0.3.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8ce81f49ae8a0482e4c55ea62ebbd7e5a686af544c00b9d090bba3ff9be97b3d" - -[[package]] -name = "float_next_after" -version = "1.0.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8bf7cc16383c4b8d58b9905a8509f02926ce3058053c056376248d958c9df1e8" - -[[package]] -name = "font-kit" -version = "0.14.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b64b34f4efd515f905952d91bc185039863705592c0c53ae6d979805dd154520" -dependencies = [ - "bitflags 2.6.0", - "byteorder", - "core-foundation", - "core-graphics", - "core-text", - "dirs", - "dwrote", - "float-ord", - "freetype-sys", - "lazy_static", - "libc", - "log", - "pathfinder_geometry", - "pathfinder_simd", - "walkdir", - "winapi", - "yeslogic-fontconfig-sys", -] - -[[package]] -name = "foreign-types" -version = "0.5.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d737d9aa519fb7b749cbc3b962edcf310a8dd1f4b67c91c4f83975dbdd17d965" -dependencies = [ - "foreign-types-macros", - "foreign-types-shared", -] - -[[package]] -name = "foreign-types-macros" -version = "0.2.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "1a5c6c585bc94aaf2c7b51dd4c2ba22680844aba4c687be581871a6f518c5742" -dependencies = [ - "proc-macro2", - "quote", - "syn", -] - -[[package]] -name = "foreign-types-shared" -version = "0.3.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "aa9a19cbb55df58761df49b23516a86d432839add4af60fc256da840f66ed35b" - -[[package]] -name = "freetype-sys" -version = "0.20.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0e7edc5b9669349acfda99533e9e0bcf26a51862ab43b08ee7745c55d28eb134" -dependencies = [ - "cc", - "libc", - "pkg-config", -] - -[[package]] -name = "geo" -version = "0.29.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "34f0e6e028c581e82e6822a68869514e94c25e7f8ea669a2d8595bdf7461ccc5" -dependencies = [ - "earcutr", - "float_next_after", - "geo-types", - "geographiclib-rs", - "i_overlay", - "log", - "num-traits", - "robust", - "rstar", - "spade", -] - -[[package]] -name = "geo-types" -version = "0.7.14" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b6f47c611187777bbca61ea7aba780213f5f3441fd36294ab333e96cfa791b65" -dependencies = [ - "approx", - "num-traits", - "rayon", - "rstar", - "serde", -] - -[[package]] -name = "geographiclib-rs" -version = "0.2.4" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e6e5ed84f8089c70234b0a8e0aedb6dc733671612ddc0d37c6066052f9781960" -dependencies = [ - "libm", -] - -[[package]] -name = "getrandom" -version = "0.2.15" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c4567c8db10ae91089c99af84c68c38da3ec2f087c3f82960bcdbf3656b6f4d7" -dependencies = [ - "cfg-if", - "libc", - "wasi", -] - -[[package]] -name = "gif" -version = "0.12.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "80792593675e051cf94a4b111980da2ba60d4a83e43e0048c5693baab3977045" -dependencies = [ - "color_quant", - "weezl", -] - -[[package]] -name = "hash32" -version = "0.3.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "47d60b12902ba28e2730cd37e95b8c9223af2808df9e902d4df49588d1470606" -dependencies = [ - "byteorder", -] - -[[package]] -name = "hashbrown" -version = "0.14.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e5274423e17b7c9fc20b6e7e208532f9b19825d82dfd615708b70edd83df41f1" -dependencies = [ - "ahash", - "allocator-api2", -] - -[[package]] -name = "hashbrown" -version = "0.15.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "bf151400ff0baff5465007dd2f3e717f3fe502074ca563069ce3a6629d07b289" - -[[package]] -name = "heapless" -version = "0.8.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0bfb9eb618601c89945a70e254898da93b13be0388091d42117462b265bb3fad" -dependencies = [ - "hash32", - "stable_deref_trait", -] - -[[package]] -name = "heck" -version = "0.5.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "2304e00983f87ffb38b55b444b5e3b60a884b5d30c0fca7d82fe33449bbe55ea" - -[[package]] -name = "i_float" -version = "1.6.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "775f9961a8d2f879725da8aff789bb20a3ddf297473e0c90af75e69313919490" -dependencies = [ - "serde", -] - -[[package]] -name = "i_key_sort" -version = "0.2.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "347c253b4748a1a28baf94c9ce133b6b166f08573157e05afe718812bc599fcd" - -[[package]] -name = "i_overlay" -version = "1.9.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "06740cd31c1f963823e007d8e6edcd2db634b2856f4f613e3df01737fd852482" -dependencies = [ - "i_float", - "i_key_sort", - "i_shape", - "i_tree", - "rayon", -] - -[[package]] -name = "i_shape" -version = "1.6.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "27dbe9e5238d6b9c694c08415bf00fb370b089949bd818ab01f41f8927b8774c" -dependencies = [ - "i_float", - "serde", -] - -[[package]] -name = "i_tree" -version = "0.8.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "155181bc97d770181cf9477da51218a19ee92a8e5be642e796661aee2b601139" - -[[package]] -name = "iana-time-zone" -version = "0.1.61" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "235e081f3925a06703c2d0117ea8b91f042756fd6e7a6e5d901e8ca1a996b220" -dependencies = [ - "android_system_properties", - "core-foundation-sys", - "iana-time-zone-haiku", - "js-sys", - "wasm-bindgen", - "windows-core", -] - -[[package]] -name = "iana-time-zone-haiku" -version = "0.1.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f31827a206f56af32e590ba56d5d2d085f558508192593743f16b2306495269f" -dependencies = [ - "cc", -] - -[[package]] -name = "image" -version = "0.24.9" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5690139d2f55868e080017335e4b94cb7414274c74f1669c84fb5feba2c9f69d" -dependencies = [ - "bytemuck", - "byteorder", - "color_quant", - "jpeg-decoder", - "num-traits", - "png", -] - -[[package]] -name = "indexmap" -version = "2.7.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "62f822373a4fe84d4bb149bf54e584a7f4abec90e072ed49cda0edea5b95471f" -dependencies = [ - "equivalent", - "hashbrown 0.15.2", -] - -[[package]] -name = "indoc" -version = "2.0.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b248f5224d1d606005e02c97f5aa4e88eeb230488bcc03bc9ca4d7991399f2b5" - -[[package]] -name = "itertools" -version = "0.11.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b1c173a5686ce8bfa551b3563d0c2170bf24ca44da99c7ca4bfdab5418c3fe57" -dependencies = [ - "either", -] - -[[package]] -name = "itertools" -version = "0.13.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "413ee7dfc52ee1a4949ceeb7dbc8a33f2d6c088194d9f922fb8318faf1f01186" -dependencies = [ - "either", -] - -[[package]] -name = "jpeg-decoder" -version = "0.3.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f5d4a7da358eff58addd2877a45865158f0d78c911d43a5784ceb7bbf52833b0" - -[[package]] -name = "js-sys" -version = "0.3.76" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "6717b6b5b077764fb5966237269cb3c64edddde4b14ce42647430a78ced9e7b7" -dependencies = [ - "once_cell", - "wasm-bindgen", -] - -[[package]] -name = "keyed_priority_queue" -version = "0.4.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "4ee7893dab2e44ae5f9d0173f26ff4aa327c10b01b06a72b52dd9405b628640d" -dependencies = [ - "indexmap", -] - -[[package]] -name = "lazy_static" -version = "1.5.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "bbd2bcb4c963f2ddae06a2efc7e9f3591312473c50c6685e1f298068316e66fe" - -[[package]] -name = "libc" -version = "0.2.168" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5aaeb2981e0606ca11d79718f8bb01164f1d6ed75080182d3abf017e6d244b6d" - -[[package]] -name = "libloading" -version = "0.8.6" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "fc2f4eb4bc735547cfed7c0a4922cbd04a4655978c09b54f1f7b228750664c34" -dependencies = [ - "cfg-if", - "windows-targets 0.52.6", -] - -[[package]] -name = "libm" -version = "0.2.11" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8355be11b20d696c8f18f6cc018c4e372165b1fa8126cef092399c9951984ffa" - -[[package]] -name = "libredox" -version = "0.1.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c0ff37bd590ca25063e35af745c343cb7a0271906fb7b37e4813e8f79f00268d" -dependencies = [ - "bitflags 2.6.0", - "libc", -] - -[[package]] -name = "log" -version = "0.4.22" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a7a70ba024b9dc04c27ea2f0c0548feb474ec5c54bba33a7f72f873a39d07b24" - -[[package]] -name = "memoffset" -version = "0.9.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "488016bfae457b036d996092f6cb448677611ce4449e970ceaf42695203f218a" -dependencies = [ - "autocfg", -] - -[[package]] -name = "miniz_oxide" -version = "0.8.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e2d80299ef12ff69b16a84bb182e3b9df68b5a91574d3d4fa6e41b65deec4df1" -dependencies = [ - "adler2", - "simd-adler32", -] - -[[package]] -name = "num-traits" -version = "0.2.19" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "071dfc062690e90b734c0b2273ce72ad0ffa95f0c74596bc250dcfd960262841" -dependencies = [ - "autocfg", - "libm", -] - -[[package]] -name = "once_cell" -version = "1.20.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "1261fe7e33c73b354eab43b1273a57c8f967d0391e80353e51f764ac02cf6775" - -[[package]] -name = "option-ext" -version = "0.2.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "04744f49eae99ab78e0d5c0b603ab218f515ea8cfe5a456d7629ad883a3b6e7d" - -[[package]] -name = "ordered-float" -version = "4.5.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c65ee1f9701bf938026630b455d5315f490640234259037edb259798b3bcf85e" -dependencies = [ - "num-traits", -] - -[[package]] -name = "pathfinder_geometry" -version = "0.5.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0b7b7e7b4ea703700ce73ebf128e1450eb69c3a8329199ffbfb9b2a0418e5ad3" -dependencies = [ - "log", - "pathfinder_simd", -] - -[[package]] -name = "pathfinder_simd" -version = "0.5.4" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5cf07ef4804cfa9aea3b04a7bbdd5a40031dbb6b4f2cbaf2b011666c80c5b4f2" -dependencies = [ - "rustc_version", -] - -[[package]] -name = "petgraph" -version = "0.6.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b4c5cc86750666a3ed20bdaf5ca2a0344f9c67674cae0515bec2da16fbaa47db" -dependencies = [ - "fixedbitset", - "indexmap", -] - -[[package]] -name = "pkg-config" -version = "0.3.31" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "953ec861398dccce10c670dfeaf3ec4911ca479e9c02154b3a215178c5f566f2" - -[[package]] -name = "plotters" -version = "0.3.7" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5aeb6f403d7a4911efb1e33402027fc44f29b5bf6def3effcc22d7bb75f2b747" -dependencies = [ - "chrono", - "font-kit", - "image", - "lazy_static", - "num-traits", - "pathfinder_geometry", - "plotters-backend", - "plotters-bitmap", - "plotters-svg", - "ttf-parser", - "wasm-bindgen", - "web-sys", -] - -[[package]] -name = "plotters-backend" -version = "0.3.7" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "df42e13c12958a16b3f7f4386b9ab1f3e7933914ecea48da7139435263a4172a" - -[[package]] -name = "plotters-bitmap" -version = "0.3.7" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "72ce181e3f6bf82d6c1dc569103ca7b1bd964c60ba03d7e6cdfbb3e3eb7f7405" -dependencies = [ - "gif", - "image", - "plotters-backend", -] - -[[package]] -name = "plotters-svg" -version = "0.3.7" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "51bae2ac328883f7acdfea3d66a7c35751187f870bc81f94563733a154d7a670" -dependencies = [ - "plotters-backend", -] - -[[package]] -name = "png" -version = "0.17.15" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b67582bd5b65bdff614270e2ea89a1cf15bef71245cc1e5f7ea126977144211d" -dependencies = [ - "bitflags 1.3.2", - "crc32fast", - "fdeflate", - "flate2", - "miniz_oxide", -] - -[[package]] -name = "portable-atomic" -version = "1.10.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "280dc24453071f1b63954171985a0b0d30058d287960968b9b2aca264c8d4ee6" - -[[package]] -name = "ppv-lite86" -version = "0.2.20" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "77957b295656769bb8ad2b6a6b09d897d94f05c41b069aede1fcdaa675eaea04" -dependencies = [ - "zerocopy", -] - -[[package]] -name = "proc-macro2" -version = "1.0.92" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "37d3544b3f2748c54e147655edb5025752e2303145b5aefb3c3ea2c78b973bb0" -dependencies = [ - "unicode-ident", -] - -[[package]] -name = "pyo3" -version = "0.23.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e484fd2c8b4cb67ab05a318f1fd6fa8f199fcc30819f08f07d200809dba26c15" -dependencies = [ - "cfg-if", - "indoc", - "libc", - "memoffset", - "once_cell", - "portable-atomic", - "pyo3-build-config", - "pyo3-ffi", - "pyo3-macros", - "unindent", -] - -[[package]] -name = "pyo3-build-config" -version = "0.23.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "dc0e0469a84f208e20044b98965e1561028180219e35352a2afaf2b942beff3b" -dependencies = [ - "once_cell", - "target-lexicon", -] - -[[package]] -name = "pyo3-ffi" -version = "0.23.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "eb1547a7f9966f6f1a0f0227564a9945fe36b90da5a93b3933fc3dc03fae372d" -dependencies = [ - "libc", - "pyo3-build-config", -] - -[[package]] -name = "pyo3-macros" -version = "0.23.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "fdb6da8ec6fa5cedd1626c886fc8749bdcbb09424a86461eb8cdf096b7c33257" -dependencies = [ - "proc-macro2", - "pyo3-macros-backend", - "quote", - "syn", -] - -[[package]] -name = "pyo3-macros-backend" -version = "0.23.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "38a385202ff5a92791168b1136afae5059d3ac118457bb7bc304c197c2d33e7d" -dependencies = [ - "heck", - "proc-macro2", - "pyo3-build-config", - "quote", - "syn", -] - -[[package]] -name = "quote" -version = "1.0.37" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b5b9d34b8991d19d98081b46eacdd8eb58c6f2b201139f7c5f643cc155a633af" -dependencies = [ - "proc-macro2", -] - -[[package]] -name = "rand" -version = "0.8.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "34af8d1a0e25924bc5b7c43c079c942339d8f0a8b57c39049bef581b46327404" -dependencies = [ - "libc", - "rand_chacha", - "rand_core", -] - -[[package]] -name = "rand_chacha" -version = "0.3.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e6c10a63a0fa32252be49d21e7709d4d4baf8d231c2dbce1eaa8141b9b127d88" -dependencies = [ - "ppv-lite86", - "rand_core", -] - -[[package]] -name = "rand_core" -version = "0.6.4" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ec0be4795e2f6a28069bec0b5ff3e2ac9bafc99e6a9a7dc3547996c5c816922c" -dependencies = [ - "getrandom", -] - -[[package]] -name = "rayon" -version = "1.10.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b418a60154510ca1a002a752ca9714984e21e4241e804d32555251faf8b78ffa" -dependencies = [ - "either", - "rayon-core", -] - -[[package]] -name = "rayon-core" -version = "1.12.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "1465873a3dfdaa8ae7cb14b4383657caab0b3e8a0aa9ae8e04b044854c8dfce2" -dependencies = [ - "crossbeam-deque", - "crossbeam-utils", -] - -[[package]] -name = "redox_users" -version = "0.4.6" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ba009ff324d1fc1b900bd1fdb31564febe58a8ccc8a6fdbb93b543d33b13ca43" -dependencies = [ - "getrandom", - "libredox", - "thiserror", -] - -[[package]] -name = "robust" -version = "1.1.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "cbf4a6aa5f6d6888f39e980649f3ad6b666acdce1d78e95b8a2cb076e687ae30" - -[[package]] -name = "rstar" -version = "0.12.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "421400d13ccfd26dfa5858199c30a5d76f9c54e0dba7575273025b43c5175dbb" -dependencies = [ - "heapless", - "num-traits", - "smallvec", -] - -[[package]] -name = "rustc_version" -version = "0.4.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "cfcb3a22ef46e85b45de6ee7e79d063319ebb6594faafcf1c225ea92ab6e9b92" -dependencies = [ - "semver", -] - -[[package]] -name = "same-file" -version = "1.0.6" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "93fc1dc3aaa9bfed95e02e6eadabb4baf7e3078b0bd1b4d7b6b0b68378900502" -dependencies = [ - "winapi-util", -] - -[[package]] -name = "semver" -version = "1.0.23" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "61697e0a1c7e512e84a621326239844a24d8207b4669b41bc18b32ea5cbf988b" - -[[package]] -name = "serde" -version = "1.0.216" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0b9781016e935a97e8beecf0c933758c97a5520d32930e460142b4cd80c6338e" -dependencies = [ - "serde_derive", -] - -[[package]] -name = "serde_derive" -version = "1.0.216" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "46f859dbbf73865c6627ed570e78961cd3ac92407a2d117204c49232485da55e" -dependencies = [ - "proc-macro2", - "quote", - "syn", -] - -[[package]] -name = "shlex" -version = "1.3.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0fda2ff0d084019ba4d7c6f371c95d8fd75ce3524c3cb8fb653a3023f6323e64" - -[[package]] -name = "simd-adler32" -version = "0.3.7" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d66dc143e6b11c1eddc06d5c423cfc97062865baf299914ab64caa38182078fe" - -[[package]] -name = "smallvec" -version = "1.13.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3c5e1a9a646d36c3599cd173a41282daf47c44583ad367b8e6837255952e5c67" - -[[package]] -name = "spade" -version = "2.12.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "93f5ef1f863aca7d1d7dda7ccfc36a0a4279bd6d3c375176e5e0712e25cb4889" -dependencies = [ - "hashbrown 0.14.5", - "num-traits", - "robust", - "smallvec", -] - -[[package]] -name = "stable_deref_trait" -version = "1.2.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a8f112729512f8e442d81f95a8a7ddf2b7c6b8a1a6f509a95864142b30cab2d3" - -[[package]] -name = "syn" -version = "2.0.90" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "919d3b74a5dd0ccd15aeb8f93e7006bd9e14c295087c9896a110f490752bcf31" -dependencies = [ - "proc-macro2", - "quote", - "unicode-ident", -] - -[[package]] -name = "target-lexicon" -version = "0.12.16" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "61c41af27dd6d1e27b1b16b489db798443478cef1f06a660c96db617ba5de3b1" - -[[package]] -name = "thiserror" -version = "1.0.69" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b6aaf5339b578ea85b50e080feb250a3e8ae8cfcdff9a461c9ec2904bc923f52" -dependencies = [ - "thiserror-impl", -] - -[[package]] -name = "thiserror-impl" -version = "1.0.69" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "4fee6c4efc90059e10f81e6d42c60a18f76588c3d74cb83a0b242a2b6c7504c1" -dependencies = [ - "proc-macro2", - "quote", - "syn", -] - -[[package]] -name = "ttf-parser" -version = "0.20.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "17f77d76d837a7830fe1d4f12b7b4ba4192c1888001c7164257e4bc6d21d96b4" - -[[package]] -name = "unicode-ident" -version = "1.0.14" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "adb9e6ca4f869e1180728b7950e35922a7fc6397f7b641499e8f3ef06e50dc83" - -[[package]] -name = "unindent" -version = "0.2.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c7de7d73e1754487cb58364ee906a499937a0dfabd86bcb980fa99ec8c8fa2ce" - -[[package]] -name = "version_check" -version = "0.9.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0b928f33d975fc6ad9f86c8f283853ad26bdd5b10b7f1542aa2fa15e2289105a" - -[[package]] -name = "walkdir" -version = "2.5.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "29790946404f91d9c5d06f9874efddea1dc06c5efe94541a7d6863108e3a5e4b" -dependencies = [ - "same-file", - "winapi-util", -] - -[[package]] -name = "wasi" -version = "0.11.0+wasi-snapshot-preview1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9c8d87e72b64a3b4db28d11ce29237c246188f4f51057d65a7eab63b7987e423" - -[[package]] -name = "wasm-bindgen" -version = "0.2.99" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a474f6281d1d70c17ae7aa6a613c87fce69a127e2624002df63dcb39d6cf6396" -dependencies = [ - "cfg-if", - "once_cell", - "wasm-bindgen-macro", -] - -[[package]] -name = "wasm-bindgen-backend" -version = "0.2.99" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5f89bb38646b4f81674e8f5c3fb81b562be1fd936d84320f3264486418519c79" -dependencies = [ - "bumpalo", - "log", - "proc-macro2", - "quote", - "syn", - "wasm-bindgen-shared", -] - -[[package]] -name = "wasm-bindgen-macro" -version = "0.2.99" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "2cc6181fd9a7492eef6fef1f33961e3695e4579b9872a6f7c83aee556666d4fe" -dependencies = [ - "quote", - "wasm-bindgen-macro-support", -] - -[[package]] -name = "wasm-bindgen-macro-support" -version = "0.2.99" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "30d7a95b763d3c45903ed6c81f156801839e5ee968bb07e534c44df0fcd330c2" -dependencies = [ - "proc-macro2", - "quote", - "syn", - "wasm-bindgen-backend", - "wasm-bindgen-shared", -] - -[[package]] -name = "wasm-bindgen-shared" -version = "0.2.99" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "943aab3fdaaa029a6e0271b35ea10b72b943135afe9bffca82384098ad0e06a6" - -[[package]] -name = "web-sys" -version = "0.3.76" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "04dd7223427d52553d3702c004d3b2fe07c148165faa56313cb00211e31c12bc" -dependencies = [ - "js-sys", - "wasm-bindgen", -] - -[[package]] -name = "weezl" -version = "0.1.8" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "53a85b86a771b1c87058196170769dd264f66c0782acf1ae6cc51bfd64b39082" - -[[package]] -name = "winapi" -version = "0.3.9" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5c839a674fcd7a98952e593242ea400abe93992746761e38641405d28b00f419" -dependencies = [ - "winapi-i686-pc-windows-gnu", - "winapi-x86_64-pc-windows-gnu", -] - -[[package]] -name = "winapi-i686-pc-windows-gnu" -version = "0.4.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ac3b87c63620426dd9b991e5ce0329eff545bccbbb34f3be09ff6fb6ab51b7b6" - -[[package]] -name = "winapi-util" -version = "0.1.9" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "cf221c93e13a30d793f7645a0e7762c55d169dbb0a49671918a2319d289b10bb" -dependencies = [ - "windows-sys 0.59.0", -] - -[[package]] -name = "winapi-x86_64-pc-windows-gnu" -version = "0.4.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "712e227841d057c1ee1cd2fb22fa7e5a5461ae8e48fa2ca79ec42cfc1931183f" - -[[package]] -name = "windows-core" -version = "0.52.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "33ab640c8d7e35bf8ba19b884ba838ceb4fba93a4e8c65a9059d08afcfc683d9" -dependencies = [ - "windows-targets 0.52.6", -] - -[[package]] -name = "windows-sys" -version = "0.48.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "677d2418bec65e3338edb076e806bc1ec15693c5d0104683f2efe857f61056a9" -dependencies = [ - "windows-targets 0.48.5", -] - -[[package]] -name = "windows-sys" -version = "0.59.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "1e38bc4d79ed67fd075bcc251a1c39b32a1776bbe92e5bef1f0bf1f8c531853b" -dependencies = [ - "windows-targets 0.52.6", -] - -[[package]] -name = "windows-targets" -version = "0.48.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9a2fa6e2155d7247be68c096456083145c183cbbbc2764150dda45a87197940c" -dependencies = [ - "windows_aarch64_gnullvm 0.48.5", - "windows_aarch64_msvc 0.48.5", - "windows_i686_gnu 0.48.5", - "windows_i686_msvc 0.48.5", - "windows_x86_64_gnu 0.48.5", - "windows_x86_64_gnullvm 0.48.5", - "windows_x86_64_msvc 0.48.5", -] - -[[package]] -name = "windows-targets" -version = "0.52.6" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9b724f72796e036ab90c1021d4780d4d3d648aca59e491e6b98e725b84e99973" -dependencies = [ - "windows_aarch64_gnullvm 0.52.6", - "windows_aarch64_msvc 0.52.6", - "windows_i686_gnu 0.52.6", - "windows_i686_gnullvm", - "windows_i686_msvc 0.52.6", - "windows_x86_64_gnu 0.52.6", - "windows_x86_64_gnullvm 0.52.6", - "windows_x86_64_msvc 0.52.6", -] - -[[package]] -name = "windows_aarch64_gnullvm" -version = "0.48.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "2b38e32f0abccf9987a4e3079dfb67dcd799fb61361e53e2882c3cbaf0d905d8" - -[[package]] -name = "windows_aarch64_gnullvm" -version = "0.52.6" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "32a4622180e7a0ec044bb555404c800bc9fd9ec262ec147edd5989ccd0c02cd3" - -[[package]] -name = "windows_aarch64_msvc" -version = "0.48.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "dc35310971f3b2dbbf3f0690a219f40e2d9afcf64f9ab7cc1be722937c26b4bc" - -[[package]] -name = "windows_aarch64_msvc" -version = "0.52.6" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "09ec2a7bb152e2252b53fa7803150007879548bc709c039df7627cabbd05d469" - -[[package]] -name = "windows_i686_gnu" -version = "0.48.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a75915e7def60c94dcef72200b9a8e58e5091744960da64ec734a6c6e9b3743e" - -[[package]] -name = "windows_i686_gnu" -version = "0.52.6" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8e9b5ad5ab802e97eb8e295ac6720e509ee4c243f69d781394014ebfe8bbfa0b" - -[[package]] -name = "windows_i686_gnullvm" -version = "0.52.6" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0eee52d38c090b3caa76c563b86c3a4bd71ef1a819287c19d586d7334ae8ed66" - -[[package]] -name = "windows_i686_msvc" -version = "0.48.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8f55c233f70c4b27f66c523580f78f1004e8b5a8b659e05a4eb49d4166cca406" - -[[package]] -name = "windows_i686_msvc" -version = "0.52.6" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "240948bc05c5e7c6dabba28bf89d89ffce3e303022809e73deaefe4f6ec56c66" - -[[package]] -name = "windows_x86_64_gnu" -version = "0.48.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "53d40abd2583d23e4718fddf1ebec84dbff8381c07cae67ff7768bbf19c6718e" - -[[package]] -name = "windows_x86_64_gnu" -version = "0.52.6" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "147a5c80aabfbf0c7d901cb5895d1de30ef2907eb21fbbab29ca94c5b08b1a78" - -[[package]] -name = "windows_x86_64_gnullvm" -version = "0.48.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0b7b52767868a23d5bab768e390dc5f5c55825b6d30b86c844ff2dc7414044cc" - -[[package]] -name = "windows_x86_64_gnullvm" -version = "0.52.6" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "24d5b23dc417412679681396f2b49f3de8c1473deb516bd34410872eff51ed0d" - -[[package]] -name = "windows_x86_64_msvc" -version = "0.48.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ed94fce61571a4006852b7389a063ab983c02eb1bb37b47f8272ce92d06d9538" - -[[package]] -name = "windows_x86_64_msvc" -version = "0.52.6" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "589f6da84c646204747d1270a2a5661ea66ed1cced2631d546fdfb155959f9ec" - -[[package]] -name = "wio" -version = "0.2.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5d129932f4644ac2396cb456385cbf9e63b5b30c6e8dc4820bdca4eb082037a5" -dependencies = [ - "winapi", -] - -[[package]] -name = "yeslogic-fontconfig-sys" -version = "6.0.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "503a066b4c037c440169d995b869046827dbc71263f6e8f3be6d77d4f3229dbd" -dependencies = [ - "dlib", - "once_cell", - "pkg-config", -] - -[[package]] -name = "zerocopy" -version = "0.7.35" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "1b9b4fd18abc82b8136838da5d50bae7bdea537c574d8dc1a34ed098d6c166f0" -dependencies = [ - "byteorder", - "zerocopy-derive", -] - -[[package]] -name = "zerocopy-derive" -version = "0.7.35" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "fa4f8080344d4671fb4e831a13ad1e68092748387dfc4f55e356242fae12ce3e" -dependencies = [ - "proc-macro2", - "quote", - "syn", -] diff --git a/bitbots_navigation/bitbots_path_planning/Cargo.toml b/bitbots_navigation/bitbots_path_planning/Cargo.toml deleted file mode 100644 index d1cb2b74a..000000000 --- a/bitbots_navigation/bitbots_path_planning/Cargo.toml +++ /dev/null @@ -1,20 +0,0 @@ -[package] -name = "bbpprs" -version = "0.1.0" -edition = "2021" - -# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html -[lib] -name = "bbpprs" -crate-type = ["cdylib"] - -[dependencies] -pyo3 = "0.23.1" -anyhow = "1.0.93" -geo = "0.29.2" -itertools = "0.13.0" -petgraph = "0.6.5" -plotters = "0.3.7" -rand = "0.8.5" -keyed_priority_queue = "0.4.2" -ordered-float = "4.5.0" diff --git a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/map.py b/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/map.py deleted file mode 100644 index 0ade6b421..000000000 --- a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/map.py +++ /dev/null @@ -1,118 +0,0 @@ -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 rclpy.node import Node -from ros2_numpy import numpify -from shapely import Geometry -from tf2_geometry_msgs import PointStamped, PoseWithCovarianceStamped - -CIRCLE_APPROXIMATION_SEGMENTS = 12 - - -class Map: - """ - Obstacle Map that keeps track of obstacles like the ball or other robots. - """ - - def __init__(self, node: Node, buffer: tf2.Buffer) -> None: - self.node = node - self.buffer = buffer - 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.frame: str = self.node.config.map.planning_frame - self.config_ball_diameter: float = self.node.config.map.ball_diameter - self.config_inflation_blur: int = self.node.config.map.inflation.blur - self.config_inflation_dilation: float = self.node.config.map.inflation.dilate - self.config_obstacle_value: int = self.node.config.map.obstacle_value - self.ball_obstacle_active: bool = True - self._robot_geometries: list[Geometry] = [] - self._ball_geometries: list[Geometry] = [] - self._obstacle_union: Geometry | None = None - - def obstacles(self) -> list[Geometry]: - """ - Return the obstacles in the map as Geometry objects - """ - return self._robot_geometries + (self._ball_geometries if self.ball_obstacle_active else []) - - def intersects(self, object: Geometry) -> bool: - """ - Check if an object intersects with any obstacles in the map - """ - return not self._obstacle_union.touches(object) and self._obstacle_union.intersects(object) - - def set_ball(self, ball: PoseWithCovarianceStamped) -> None: - """ - Adds a given ball to the ball buffer - """ - point = PointStamped() - point.header.frame_id = ball.header.frame_id - point.point = ball.pose.pose.position - ball_buffer = [] - try: - 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_ball_geometries(ball_buffer) - self._update_obstacle_union() - - def set_robots(self, robots: sv3dm.RobotArray): - """ - Adds a given robot array to the robot buffer - """ - new_buffer: list[sv3dm.Robot] = [] - robot: sv3dm.Robot - for robot in robots.robots: - point = PointStamped() - point.header.frame_id = robots.header.frame_id - point.point = robot.bb.center.position - try: - robot.bb.center.position = self.buffer.transform(point, self.frame).point - new_buffer.append(robot) - except (tf2.ConnectivityException, tf2.LookupException, tf2.ExtrapolationException) as e: - self.node.get_logger().warn(str(e)) - self._update_robot_geometries(new_buffer) - self._update_obstacle_union() - - def _update_robot_geometries(self, robots): - self._robot_geometries = [] - for robot in robots: - 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_dilation - geometry = center.buffer(radius + dilation, quad_segs=CIRCLE_APPROXIMATION_SEGMENTS // 4) - self._robot_geometries.append(geometry) - - def _update_ball_geometries(self, balls): - self._ball_geometries = [] - for ball in balls: - center = shapely.Point(ball.x, ball.y) - radius = self.config_ball_diameter / 2 - dilation = self.config_inflation_dilation - geometry = center.buffer(radius + dilation, quad_segs=CIRCLE_APPROXIMATION_SEGMENTS // 4) - self._ball_geometries.append(geometry) - - def _update_obstacle_union(self): - self._obstacle_union = shapely.union_all( - (self._ball_geometries if self.ball_obstacle_active else []) + self._robot_geometries - ) - - def avoid_ball(self, state: bool) -> None: - """ - Activates or deactivates the ball obstacle - """ - self.ball_obstacle_active = state - self._update_obstacle_union() - - def get_frame(self) -> str: - """ - Returns the frame of reference of the map - """ - return self.frame 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 c4c234c3c..d8e27aaab 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 @@ -13,7 +13,6 @@ from visualization_msgs.msg import MarkerArray 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 VisibilityPlanner @@ -31,23 +30,21 @@ def __init__(self) -> None: # We need to create a tf buffer self.tf_buffer = Buffer(self, Duration(seconds=self.config.tf_buffer_duration)) - # Create submodules - self.map = Map(node=self, buffer=self.tf_buffer) - self.planner = VisibilityPlanner(node=self, buffer=self.tf_buffer, map=self.map) + self.planner = VisibilityPlanner(node=self, buffer=self.tf_buffer) self.controller = Controller(node=self, buffer=self.tf_buffer) # Subscriber self.create_subscription( PoseWithCovarianceStamped, self.config.map.ball_update_topic, - self.map.set_ball, + self.planner.set_ball, 5, callback_group=MutuallyExclusiveCallbackGroup(), ) self.create_subscription( sv3dm.RobotArray, self.config.map.robot_update_topic, - self.map.set_robots, + self.planner.set_robots, 5, callback_group=MutuallyExclusiveCallbackGroup(), ) @@ -64,7 +61,7 @@ def __init__(self) -> None: self.create_subscription( Bool, "ball_obstacle_active", - lambda msg: self.map.avoid_ball(msg.data), + lambda msg: self.planner.avoid_ball(msg.data), 5, callback_group=MutuallyExclusiveCallbackGroup(), ) @@ -93,13 +90,7 @@ def step(self) -> None: try: if self.planner.active(): # Calculate the path to the goal pose considering the current map - t1 = time.time() path = self.planner.step() - t2 = time.time() - print(f"delta = {t2 - t1}") - # Publish the visibility graph for visualization - markers = self.planner.visibility_graph_wrapper() - self.graph_pub.publish(markers) # Publish the path for visualization self.path_pub.publish(path) # Calculate the command velocity to follow the given path 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 b5b8e4966..d897d8662 100644 --- a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/planner.py +++ b/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/planner.py @@ -5,16 +5,18 @@ import geometry_msgs.msg as geom_msg import rustworkx as rx import tf2_ros as tf2 +from tf2_geometry_msgs import PointStamped, PoseWithCovarianceStamped from geometry_msgs.msg import PoseStamped 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 visualization_msgs.msg import Marker, MarkerArray +import soccer_vision_3d_msgs.msg as sv3dm +from ros2_numpy import numpify -from bitbots_path_planning.map import Map +from bitbots_pathplanning_rust import ObstacleMapConfig, ObstacleMap, RoundObstacle class Planner(ABC): @@ -27,35 +29,62 @@ def cancel_goal(self) -> None: pass @abstractmethod - def active(self) -> bool: + def set_robots(self, robots: sv3dm.RobotArray) -> None: pass @abstractmethod - def step(self) -> Path: + def set_ball(self, ball: PoseWithCovarianceStamped) -> None: pass + @abstractmethod + def avoid_ball(self, state: bool) -> None: + pass -class VisibilityNode: - """ - A Node that stores all the information needed for A-Star - """ + @abstractmethod + def active(self) -> bool: + pass - 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 + @abstractmethod + def step(self) -> Path: + pass class VisibilityPlanner(Planner): - def __init__(self, node: Node, buffer: tf2.Buffer, map: Map) -> None: + def __init__(self, node: Node, buffer: tf2.Buffer) -> None: self.node = node self.buffer = buffer - self.map = map + self.robots = [] + self.ball = None self.goal: PoseStamped | None = None self.base_footprint_frame: str = self.node.config.base_footprint_frame + self.ball_obstacle_active: bool = True + self.frame: str = self.node.config.map.planning_frame + + def set_robots(self, robots: sv3dm.RobotArray): + new_buffer: list[RoundObstacle] = [] + for robot in robots.robots: + point = PointStamped() + point.header.frame_id = robots.header.frame_id + point.point = robot.bb.center.position + radius = max(numpify(robot.bb.size)[:2]) / 2 + try: + position = self.buffer.transform(point, self.frame).point + new_buffer.append(RoundObstacle(center=(position.x, position.y), radius=radius)) + except (tf2.ConnectivityException, tf2.LookupException, tf2.ExtrapolationException) as e: + self.node.get_logger().warn(str(e)) + self.robots = new_buffer + self.node.get_logger().info(f"self.robots set to {[str(r) for r in self.robots]}") + + def set_ball(self, ball: PoseWithCovarianceStamped) -> None: + point = PointStamped() + point.header.frame_id = ball.header.frame_id + point.point = ball.pose.pose.position + try: + tf_point = self.buffer.transform(point, self.frame).point + self.ball = RoundObstacle(center=(tf_point.x, tf_point.y), radius=self.node.config.map.ball_diameter) + except (tf2.ConnectivityException, tf2.LookupException, tf2.ExtrapolationException) as e: + self.ball = None + self.node.get_logger().warn(str(e)) def set_goal(self, pose: PoseStamped) -> None: """ @@ -64,6 +93,12 @@ 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 avoid_ball(self, state: bool) -> None: + """ + Activates or deactivates the ball obstacle + """ + self.ball_obstacle_active = state + def cancel_goal(self) -> None: """ Removes the current goal @@ -77,134 +112,27 @@ def active(self) -> bool: """ return self.goal is not None - def visibility_graph(self, start: Point, goal: Point) -> typing.Tuple[rx.PyGraph, int, int]: - graph = rx.PyGraph() - start_node = graph.add_node(start) - goal_node = graph.add_node(goal) - for obstacle in self.map.obstacles(): - for x, y in obstacle.boundary.coords: - graph.add_node(Point(x, y)) - for a, b in product(graph.node_indices(), repeat=2): - if a == b: - continue - a_point = graph.get_node_data(a) - b_point = graph.get_node_data(b) - if not self.map.intersects(LineString([a_point, b_point])): - graph.add_edge(a, b, distance(a_point, b_point)) - return (graph, start_node, goal_node) - - def visibility_graph_wrapper(self) -> MarkerArray: - 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) - (graph, _, _) = self.visibility_graph(start, goal) - markers = MarkerArray() - nodes = Marker(type=Marker.POINTS, ns="visibility_graph_nodes", action=Marker.ADD) - nodes.header.frame_id = self.map.get_frame() - nodes.header.stamp = self.node.get_clock().now().to_msg() - for node in graph.nodes(): - nodes.points.append(geom_msg.Point(x=node.x, y=node.y, z=0.5)) - nodes.color.a = 1.0 - nodes.color.r = 0.2 - nodes.color.g = 0.2 - nodes.color.b = 0.2 - nodes.scale.x = 0.03 - nodes.scale.y = 0.03 - nodes.scale.z = 0.03 - markers.markers.append(nodes) - edges = Marker(type=Marker.LINE_LIST, ns="visibility_graph_edges", action=Marker.ADD) - edges.header.frame_id = self.map.get_frame() - edges.header.stamp = self.node.get_clock().now().to_msg() - for edge in graph.edge_indices(): - (a, b) = graph.get_edge_endpoints_by_index(edge) - a_point = graph.get_node_data(a) - b_point = graph.get_node_data(b) - edges.points.append(geom_msg.Point(x=a_point.x, y=a_point.y, z=0.5)) - edges.points.append(geom_msg.Point(x=b_point.x, y=b_point.y, z=0.5)) - edges.color.a = 1.0 - edges.color.r = 0.2 - edges.color.g = 0.2 - edges.color.b = 0.2 - edges.scale.x = 0.015 - markers.markers.append(edges) - return markers - def step(self) -> Path: - goal = Point(self.goal.pose.position.x, self.goal.pose.position.y) + goal = (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) + self.frame, self.base_footprint_frame, Time(), Duration(seconds=0.2) ).transform.translation - start = Point(my_position.x, my_position.y) - (graph, start_node, goal_node) = self.visibility_graph(start, goal) - path = rx.astar_shortest_path( - graph, start_node, lambda node: node == goal, lambda edge: edge, lambda node: distance(node, goal) - ) - - def map_to_pose(node: int): - position = graph.get_node_data(node) + start = (my_position.x, my_position.y) + config = ObstacleMapConfig(dilate=self.node.config.map.inflation.dilate, num_vertices=12) + obstacles = self.robots + if self.ball_obstacle_active and self.ball is not None: + obstacles.append(self.ball) + omap = ObstacleMap(config, obstacles) + path = omap.shortest_path(start, goal) + + def map_to_pose(position): pose = PoseStamped() - pose.pose.position.x = position.x - pose.pose.position.y = position.y + pose.pose.position.x = position[0] + pose.pose.position.y = position[1] return pose return Path( - header=Header(frame_id=self.map.get_frame(), stamp=self.node.get_clock().now().to_msg()), + header=Header(frame_id=self.frame, stamp=self.node.get_clock().now().to_msg()), poses=list(map(map_to_pose, path)), ) - -""" - 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) - poses.append(self.goal) - 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 -""" diff --git a/bitbots_navigation/bitbots_path_planning/src/lib.rs b/bitbots_navigation/bitbots_path_planning/src/lib.rs deleted file mode 100644 index 4100ce63f..000000000 --- a/bitbots_navigation/bitbots_path_planning/src/lib.rs +++ /dev/null @@ -1,13 +0,0 @@ -use map::{Ball, ObstacleMap, ObstacleMapConfig, Robot}; -use pyo3::prelude::*; -pub mod map; -pub mod util; - -#[pymodule] -fn bbpprs(m: &Bound<'_, PyModule>) -> PyResult<()> { - m.add_class::()?; - m.add_class::()?; - m.add_class::()?; - m.add_class::()?; - Ok(()) -} diff --git a/bitbots_navigation/bitbots_path_planning/src/map.rs b/bitbots_navigation/bitbots_path_planning/src/map.rs deleted file mode 100644 index 107ec4f68..000000000 --- a/bitbots_navigation/bitbots_path_planning/src/map.rs +++ /dev/null @@ -1,245 +0,0 @@ -use std::collections::{HashMap, HashSet}; - -use geo::{ - BooleanOps, Coord, Distance, Euclidean, Intersects, Line, LineString, MultiPolygon, Polygon -}; -use keyed_priority_queue::{Entry, KeyedPriorityQueue}; -use ordered_float::OrderedFloat; -use pyo3::prelude::*; - -use crate::util::regular_ngon; - -/// A trait representing an Obstacle that can be represented as a Polygon -pub trait Obstacle { - /// Get the Polygon representation of this obstacle - fn as_polygon(&self, config: &ObstacleMapConfig) -> Polygon; - - /// Get the Polygon representation of this obstacle FOR INTERSECTION - fn as_polygon_for_intersection(&self, config: &ObstacleMapConfig) -> Polygon; -} - -/// A robot obstacle -#[pyclass(str = "Robot(center={center:?}, radius={radius})")] -#[derive(Debug, Copy, Clone)] -pub struct Robot { - #[pyo3(get, set)] - center: (f64, f64), - #[pyo3(get, set)] - radius: f64, -} - -#[pymethods] -impl Robot { - #[new] - pub fn new(center: (f64, f64), radius: f64) -> Self { - Self { center, radius } - } -} - -impl Obstacle for Robot { - fn as_polygon(&self, config: &ObstacleMapConfig) -> Polygon { - regular_ngon(self.center.into(), self.radius + config.dilate, 12) - } - - fn as_polygon_for_intersection(&self, config: &ObstacleMapConfig) -> Polygon { - regular_ngon(self.center.into(), self.radius + config.dilate - 0.01, 12) - } -} - -#[pyclass(str = "Robot(center={center:?}, radius={radius})")] -#[derive(Debug, Copy, Clone)] -pub struct Ball { - #[pyo3(get, set)] - center: (f64, f64), - #[pyo3(get, set)] - radius: f64, -} - -#[pymethods] -impl Ball { - #[new] - pub fn new(center: (f64, f64), radius: f64) -> Self { - Self { center, radius } - } -} - -impl Obstacle for Ball { - fn as_polygon(&self, config: &ObstacleMapConfig) -> Polygon { - regular_ngon(self.center.into(), self.radius + config.dilate, 12) - } - - fn as_polygon_for_intersection(&self, config: &ObstacleMapConfig) -> Polygon { - regular_ngon(self.center.into(), self.radius + config.dilate - 0.01, 12) - } -} -#[pyclass] -#[derive(Debug, Clone, Copy)] -pub struct ObstacleMapConfig { - #[pyo3(get, set)] - dilate: f64, -} - -#[pymethods] -impl ObstacleMapConfig { - #[new] - pub fn new(dilate: f64) -> Self { - Self { dilate } - } -} - -#[pyclass] -#[derive(Debug, Clone)] -pub struct ObstacleMap { - config: ObstacleMapConfig, - #[pyo3(get, set)] - ball: Option, - #[pyo3(get, set)] - robots: Vec, -} - -impl ObstacleMap{ - /// Get the list of polygonal obstacles in the map - fn polygons(&self) -> Vec { - let mut obstacles = self - .robots - .iter() - .map(|robot| robot.as_polygon(&self.config)) - .collect::>(); - if let Some(ball) = self.ball { - obstacles.push(ball.as_polygon(&self.config)); - }; - obstacles - } - - /// Get the list of polygonal obstacles in the map for correct intersection - fn polygons_for_intersection(&self) -> Vec { - let mut obstacles = self - .robots - .iter() - .map(|robot| robot.as_polygon_for_intersection(&self.config)) - .collect::>(); - if let Some(ball) = self.ball { - obstacles.push(ball.as_polygon_for_intersection(&self.config)); - }; - obstacles - } - - /// Get the visibility graph of this map - fn vertices_and_obstacles(&self, start: Coord, goal: Coord) -> (Vec, MultiPolygon) { - let polygons = self.polygons(); - let polygons_for_intersection = self.polygons_for_intersection(); - let vertices = polygons - .iter() - .map(|polygon| polygon.exterior().coords().cloned()) - .flatten() - .chain(vec![start, goal].into_iter()) - .collect::>(); - let obstacles = polygons_for_intersection.iter().fold( - MultiPolygon::new(vec![Polygon::new(LineString::new(vec![]), vec![])]), - |a, b| a.union(b), - ); - (vertices, obstacles) - } - - fn heuristic(vertices: &Vec, node: usize) -> OrderedFloat { - OrderedFloat(Euclidean::distance( - vertices[node], - vertices[vertices.len() - 1], - )) - } - - fn distance(vertices: &Vec, from: usize, to: usize) -> OrderedFloat { - OrderedFloat(Euclidean::distance(vertices[from], vertices[to])) - } -} - -#[pymethods] -impl ObstacleMap { - /// Create a new ObstacleMap with the given initial ball and robots - #[new] - #[pyo3(signature=(config, robots, ball=None))] - pub fn new(config: ObstacleMapConfig, robots: Vec, ball: Option) -> Self { - Self { - config, - ball, - robots, - } - } - - /// Get the shortest from start to goal in a given visibilty graph, which defaults to self.visibility_graph - pub fn shortest_path(&self, start: (f64, f64), goal: (f64, f64)) -> Option> { - let (vertices, obstacles) = self.vertices_and_obstacles(start.into(), goal.into()); - let mut successors = HashSet::::from_iter(0..vertices.len()); - let mut open = KeyedPriorityQueue::>::new(); - open.push( - vertices.len() - 2, - -Self::heuristic(&vertices, vertices.len() - 2), - ); - let mut from = HashMap::::new(); - let mut g_score = HashMap::>::new(); - g_score.insert(vertices.len() - 2, OrderedFloat(0.0)); - - // needed if goal is in obstacle - let mut closest_vertex = vertices.len() - 2; - let mut closest_distance = OrderedFloat(std::f64::INFINITY); - - while let Some((vertex, _)) = open.pop() { - - if Self::heuristic(&vertices, vertex) < closest_distance - 0.01 - { - closest_vertex = vertex; - closest_distance = Self::heuristic(&vertices, vertex); - }; - - if vertex == vertices.len() - 1 { - let mut path = vec![vertex]; - let mut current = vertex; - while let Some(previous) = from.get(¤t) { - current = *previous; - path.push(*previous); - } - path.reverse(); - return Some(path.into_iter().map(|idx| vertices[idx].x_y()).collect()); - } - let g_vertex = g_score - .get(&vertex) - .unwrap_or(&OrderedFloat(std::f64::INFINITY)) - .clone(); - successors.remove(&vertex); - //println!("evaluating successors to node {} (coords={:?})", vertex, vertices[vertex]); - for successor in successors.iter() { - if obstacles.intersects(&Line::new(vertices[vertex], vertices[*successor])) { - //println!(" not considering node {} (coords={:?}) because of intersection", successor, vertices[*successor]); - continue; - } - //println!(" successor={} coords={:?}", successor, vertices[*successor]); - let g_successor = g_score - .get(successor) - .unwrap_or(&OrderedFloat(std::f64::INFINITY)); - let g_tentative = g_vertex + &Self::distance(&vertices, vertex, *successor); - //println!(" successor={successor}, tentative={g_tentative}, successor={g_successor}"); - if g_tentative < *g_successor { - from.insert(*successor, vertex); - g_score.insert(*successor, g_tentative); - match open.entry(*successor) { - Entry::Occupied(entry) => { - entry.set_priority(-(g_tentative + Self::heuristic(&vertices, *successor))); - }, - Entry::Vacant(entry) => { - entry.set_priority(-(g_tentative + Self::heuristic(&vertices, *successor))); - } - }; - }; - } - } - let mut path = vec![vertices.len() - 1]; - let mut current = closest_vertex; - path.push(current); - while let Some(previous) = from.get(¤t) { - current = *previous; - path.push(*previous); - } - path.reverse(); - return Some(path.into_iter().map(|idx| vertices[idx].x_y()).collect()); - } -} diff --git a/bitbots_navigation/bitbots_path_planning/src/util.rs b/bitbots_navigation/bitbots_path_planning/src/util.rs deleted file mode 100644 index d17308847..000000000 --- a/bitbots_navigation/bitbots_path_planning/src/util.rs +++ /dev/null @@ -1,11 +0,0 @@ -use geo::{Coord, LineString, Polygon}; - -pub fn regular_ngon(center: Coord, radius: f64, sides: usize) -> Polygon { - let points = (0..sides) - .map(|side| { - let angle = (side as f64) / (sides as f64) * std::f64::consts::TAU; - center + Coord::from((radius * angle.cos(), radius * angle.sin())) - }) - .collect::>(); - Polygon::new(LineString::new(points.into()), vec![]) -} \ No newline at end of file diff --git a/requirements/common.txt b/requirements/common.txt index cb1fb1659..506afc064 100644 --- a/requirements/common.txt +++ b/requirements/common.txt @@ -3,8 +3,7 @@ pip transforms3d==0.4.1 git+https://github.com/Flova/pyastar2d git+https://github.com/bit-bots/YOEO -heapdict==1.0.1 -rustworkx==0.15.1 +git+https://github.com/bit-bots/bitbots_pathplanning_rust simpleeval # The following dependencies are only needed for rl walking and we don't actively use them currently From d12b19c698f485ce5b91ed48f0506ad0532f80a0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Helena=20J=C3=A4ger?= Date: Wed, 18 Dec 2024 18:25:57 +0100 Subject: [PATCH 23/47] changed some defaults --- bitbots_misc/bitbots_ipm/config/soccer_ipm.yaml | 4 ++-- .../bitbots_path_planning/bitbots_path_planning/planner.py | 1 + .../config/path_planning_parameters.yaml | 2 +- 3 files changed, 4 insertions(+), 3 deletions(-) diff --git a/bitbots_misc/bitbots_ipm/config/soccer_ipm.yaml b/bitbots_misc/bitbots_ipm/config/soccer_ipm.yaml index 15f4637e7..55e8a138f 100644 --- a/bitbots_misc/bitbots_ipm/config/soccer_ipm.yaml +++ b/bitbots_misc/bitbots_ipm/config/soccer_ipm.yaml @@ -20,8 +20,8 @@ soccer_ipm: robots: footpoint_out_of_image_threshold: 0.8 object_default_dimensions: - x: 0.2 - y: 0.2 + x: 0.3 + y: 0.3 z: 1.0 output_frame: 'base_footprint' 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 d897d8662..bd3453914 100644 --- a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/planner.py +++ b/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/planner.py @@ -118,6 +118,7 @@ def step(self) -> Path: self.frame, self.base_footprint_frame, Time(), Duration(seconds=0.2) ).transform.translation start = (my_position.x, my_position.y) + self.node.get_logger().info(f"got dilation parameter: {self.node.config.map.inflation.dilate}") config = ObstacleMapConfig(dilate=self.node.config.map.inflation.dilate, num_vertices=12) obstacles = self.robots if self.ball_obstacle_active and self.ball is not None: diff --git a/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml b/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml index 6e9ed58e9..7b17d6b49 100644 --- a/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml +++ b/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml @@ -61,7 +61,7 @@ bitbots_path_planning: inflation: dilate: type: double - default_value: 0.25 + default_value: 0.35 description: 'The dilation value for the inflation' read_only: true blur: From 0d7df461b119648f73f7eb7568d694209afab281 Mon Sep 17 00:00:00 2001 From: Lea Date: Fri, 20 Dec 2024 21:46:45 +0100 Subject: [PATCH 24/47] changed config parameters to reasonable values --- bitbots_misc/bitbots_ipm/config/soccer_ipm.yaml | 4 ++-- .../config/path_planning_parameters.yaml | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/bitbots_misc/bitbots_ipm/config/soccer_ipm.yaml b/bitbots_misc/bitbots_ipm/config/soccer_ipm.yaml index 55e8a138f..21519c932 100644 --- a/bitbots_misc/bitbots_ipm/config/soccer_ipm.yaml +++ b/bitbots_misc/bitbots_ipm/config/soccer_ipm.yaml @@ -20,8 +20,8 @@ soccer_ipm: robots: footpoint_out_of_image_threshold: 0.8 object_default_dimensions: - x: 0.3 - y: 0.3 + x: 0.5 + y: 0.5 z: 1.0 output_frame: 'base_footprint' diff --git a/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml b/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml index 7b17d6b49..6e9ed58e9 100644 --- a/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml +++ b/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml @@ -61,7 +61,7 @@ bitbots_path_planning: inflation: dilate: type: double - default_value: 0.35 + default_value: 0.25 description: 'The dilation value for the inflation' read_only: true blur: From c67464d0129dd195218a7eb867cb5e60ab6196a7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Helena=20J=C3=A4ger?= Date: Wed, 15 Jan 2025 14:30:47 +0100 Subject: [PATCH 25/47] timing stuff --- .../bitbots_path_planning/path_planning.py | 11 +++++++++++ .../bitbots_path_planning/planner.py | 2 -- 2 files changed, 11 insertions(+), 2 deletions(-) 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 d8e27aaab..83c6305e8 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 @@ -16,6 +16,7 @@ from bitbots_path_planning.path_planning_parameters import bitbots_path_planning as parameters from bitbots_path_planning.planner import VisibilityPlanner +from time import time class PathPlanning(Node): """ @@ -33,6 +34,8 @@ def __init__(self) -> None: self.planner = VisibilityPlanner(node=self, buffer=self.tf_buffer) self.controller = Controller(node=self, buffer=self.tf_buffer) + self.measurements = dict() + # Subscriber self.create_subscription( PoseWithCovarianceStamped, @@ -90,7 +93,15 @@ def step(self) -> None: try: if self.planner.active(): # Calculate the path to the goal pose considering the current map + start = time() path = self.planner.step() + end = time() + if len(self.planner.robots) not in self.measurements: + self.measurements[len(self.planner.robots)] = (1, end - start) + else: + (n, avg) = self.measurements[len(self.planner.robots)] + self.measurements[len(self.planner.robots)] = (n + 1, (n * avg + (end - start)) / (n + 1)) + self.get_logger().info(f"planning steps={self.measurements}") # Publish the path for visualization self.path_pub.publish(path) # Calculate the command velocity to follow the given path 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 bd3453914..0c06147d8 100644 --- a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/planner.py +++ b/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/planner.py @@ -73,7 +73,6 @@ def set_robots(self, robots: sv3dm.RobotArray): except (tf2.ConnectivityException, tf2.LookupException, tf2.ExtrapolationException) as e: self.node.get_logger().warn(str(e)) self.robots = new_buffer - self.node.get_logger().info(f"self.robots set to {[str(r) for r in self.robots]}") def set_ball(self, ball: PoseWithCovarianceStamped) -> None: point = PointStamped() @@ -118,7 +117,6 @@ def step(self) -> Path: self.frame, self.base_footprint_frame, Time(), Duration(seconds=0.2) ).transform.translation start = (my_position.x, my_position.y) - self.node.get_logger().info(f"got dilation parameter: {self.node.config.map.inflation.dilate}") config = ObstacleMapConfig(dilate=self.node.config.map.inflation.dilate, num_vertices=12) obstacles = self.robots if self.ball_obstacle_active and self.ball is not None: From 3fbc4f77308de941df697661ccfc8df5a82b2c43 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Helena=20J=C3=A4ger?= Date: Wed, 15 Jan 2025 16:53:57 +0100 Subject: [PATCH 26/47] changed best-effort strategy --- Cargo.lock | 2 +- Cargo.toml | 2 +- src/planner.rs | 26 ++++++++++++++------------ 3 files changed, 16 insertions(+), 14 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index dc316f713..c6834e261 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -37,7 +37,7 @@ checksum = "ace50bade8e6234aa140d9a2f552bbee1db4d353f69b8217bc503490fc1a9f26" [[package]] name = "bitbots_pathplanning_rust" -version = "0.1.0" +version = "0.1.1" dependencies = [ "geo", "keyed_priority_queue", diff --git a/Cargo.toml b/Cargo.toml index 39c62ef28..3bbe07168 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "bitbots_pathplanning_rust" -version = "0.1.0" +version = "0.1.1" edition = "2021" # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html diff --git a/src/planner.rs b/src/planner.rs index 2e516380b..a368b8528 100644 --- a/src/planner.rs +++ b/src/planner.rs @@ -18,6 +18,9 @@ pub struct PathPlanner { from: HashMap, } +const MULTIPLIER: f64 = 10.0; + + impl PathPlanner { pub fn new(map: &ObstacleMap, start: (f64, f64), goal: (f64, f64)) -> Self { let mut vertices = map.as_vertices(); @@ -78,11 +81,17 @@ impl PathPlanner { .unwrap_or(&OrderedFloat(std::f64::INFINITY)) .clone(); for successor in self.successors.iter().cloned() { - if self.obstacle.intersects(&Line::new(self.vertices[vertex], self.vertices[successor])) { - continue; - } + let multiplier = if self.obstacle.intersects(&Line::new(self.vertices[vertex], self.vertices[successor])) { + if self.goal == successor || self.start == vertex { + OrderedFloat(MULTIPLIER) + } else { + continue; + } + } else { + OrderedFloat(1.0) + }; let g_successor = self.g_score.get(&successor).unwrap_or(&OrderedFloat(std::f64::INFINITY)).clone(); - let g_tentative = g_vertex + self.distance(vertex, successor); + let g_tentative = g_vertex + multiplier * self.distance(vertex, successor); if g_tentative < g_successor { self.from.insert(successor, vertex); self.g_score.insert(successor, g_tentative); @@ -100,20 +109,13 @@ impl PathPlanner { } pub fn shortest_path(mut self) -> Vec<(f64, f64)> { - let mut closest_vertex = self.start; - let mut closest_distance = OrderedFloat(std::f64::INFINITY); while let Some((vertex, _)) = self.open.pop() { - if self.heuristic(vertex) < closest_distance - EPSILON { - closest_vertex = vertex; - closest_distance = self.heuristic(vertex); - } if vertex == self.goal { return self.reconstruct_path(vertex); } self.successors.remove(&vertex); self.expand_node(vertex); } - self.from.insert(self.goal, closest_vertex); - self.reconstruct_path(self.goal) + unreachable!() } } From 5be1b1ad2e292b1bdb3d7bfa43f35b92bfae3e29 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Helena=20J=C3=A4ger?= Date: Wed, 15 Jan 2025 17:20:17 +0100 Subject: [PATCH 27/47] changed default params --- .../bitbots_path_planning/planner.py | 13 ++++++++----- .../config/path_planning_parameters.yaml | 6 +++--- 2 files changed, 11 insertions(+), 8 deletions(-) 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 235e1a660..2a5cf66a5 100644 --- a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/planner.py +++ b/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/planner.py @@ -45,7 +45,7 @@ def step(self) -> Path: class VisibilityPlanner(Planner): - def __init__(self, node: NodeWithConfig, buffer: tf2.Buffer) -> None: + def __init__(self, node: NodeWithConfig, buffer: tf2.BufferInterface) -> None: self.node = node self.buffer = buffer self.robots = [] @@ -75,7 +75,7 @@ def set_ball(self, ball: PoseWithCovarianceStamped) -> None: point.point = ball.pose.pose.position try: tf_point = self.buffer.transform(point, self.frame).point - self.ball = RoundObstacle(center=(tf_point.x, tf_point.y), radius=self.node.config.map.ball_diameter) + self.ball = RoundObstacle(center=(tf_point.x, tf_point.y), radius=self.node.config.map.ball_diameter / 2.0) except (tf2.ConnectivityException, tf2.LookupException, tf2.ExtrapolationException) as e: self.ball = None self.node.get_logger().warn(str(e)) @@ -113,8 +113,8 @@ def step(self) -> Path: ).transform.translation start = (my_position.x, my_position.y) config = ObstacleMapConfig(dilate=self.node.config.map.inflation.dilate, num_vertices=12) - obstacles = self.robots - if self.ball_obstacle_active and self.ball is not None: + obstacles = self.robots.copy() + if self.ball is not None: obstacles.append(self.ball) omap = ObstacleMap(config, obstacles) path = omap.shortest_path(start, goal) @@ -124,8 +124,11 @@ def map_to_pose(position): pose.pose.position.x = position[0] pose.pose.position.y = position[1] return pose + + poses = list(map(map_to_pose, path)) + poses.append(self.goal) return Path( header=Header(frame_id=self.frame, stamp=self.node.get_clock().now().to_msg()), - poses=list(map(map_to_pose, path)), + poses=poses, ) diff --git a/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml b/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml index a858f1219..8fd02d0b2 100644 --- a/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml +++ b/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml @@ -67,7 +67,7 @@ bitbots_path_planning: inflation: dilate: type: double - default_value: 0.25 + default_value: 0.3 description: 'The dilation value for the inflation' read_only: true blur: @@ -79,7 +79,7 @@ bitbots_path_planning: controller: carrot_distance: type: int - default_value: 4 + default_value: 1 description: 'The distance to the carrot that we want to reach on the path' validation: bounds<>: [0, 100] @@ -109,7 +109,7 @@ bitbots_path_planning: bounds<>: [0.0, 1.0] smoothing_tau: type: double - default_value: 1.0 + default_value: 0.1 description: 'This is the time constant of the exponential smoothing filter. The higher the value, the more smoothing is applied. It denotes the time after which a unit step function input signal reaches 63% (1-1/e) of its original strength. In other words, it denotes the time it takes for a new input to be 63% integrated into the output. It is update rate independent!' validation: bounds<>: [0.0, 1.0] From 4180784c01add2fb7467d57adf147f308738659d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Helena=20J=C3=A4ger?= Date: Wed, 15 Jan 2025 17:31:59 +0100 Subject: [PATCH 28/47] removed unused import --- Cargo.lock | 2 +- Cargo.toml | 2 +- src/planner.rs | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index c6834e261..2203a6978 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -37,7 +37,7 @@ checksum = "ace50bade8e6234aa140d9a2f552bbee1db4d353f69b8217bc503490fc1a9f26" [[package]] name = "bitbots_pathplanning_rust" -version = "0.1.1" +version = "0.1.2" dependencies = [ "geo", "keyed_priority_queue", diff --git a/Cargo.toml b/Cargo.toml index 3bbe07168..0fae859f3 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "bitbots_pathplanning_rust" -version = "0.1.1" +version = "0.1.2" edition = "2021" # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html diff --git a/src/planner.rs b/src/planner.rs index a368b8528..18b5e46d1 100644 --- a/src/planner.rs +++ b/src/planner.rs @@ -4,7 +4,7 @@ use geo::{Coord, Distance, Euclidean, Intersects, Line, MultiPolygon}; use keyed_priority_queue::{Entry, KeyedPriorityQueue}; use ordered_float::OrderedFloat; -use crate::{map::ObstacleMap, obstacle::EPSILON}; +use crate::map::ObstacleMap; #[derive(Debug, Clone)] pub struct PathPlanner { From a83a8d602d51edeb9f4028eca34dce931f04190f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?P=C3=A4r=20Wiegmann?= Date: Wed, 22 Jan 2025 17:46:53 +0100 Subject: [PATCH 29/47] Add missing dep --- bitbots_navigation/bitbots_path_planning/package.xml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/bitbots_navigation/bitbots_path_planning/package.xml b/bitbots_navigation/bitbots_path_planning/package.xml index 6634a76ee..b16edc786 100644 --- a/bitbots_navigation/bitbots_path_planning/package.xml +++ b/bitbots_navigation/bitbots_path_planning/package.xml @@ -8,8 +8,9 @@ MIT bitbots_tf_buffer - geometry_msgs + cargo generate_parameter_library + geometry_msgs nav_msgs python3-numpy python3-opencv From 2d9cff33bb9f0b690abf1d2920b2c017ee880bec Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?P=C3=A4r=20Wiegmann?= Date: Wed, 22 Jan 2025 15:38:48 +0100 Subject: [PATCH 30/47] Add bitbots tf buffer to sync includes --- sync_includes_wolfgang_nuc.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sync_includes_wolfgang_nuc.yaml b/sync_includes_wolfgang_nuc.yaml index 7c2e24c43..9f8005637 100644 --- a/sync_includes_wolfgang_nuc.yaml +++ b/sync_includes_wolfgang_nuc.yaml @@ -15,7 +15,6 @@ include: - bitbots_parameter_blackboard - bitbots_robot_description - bitbots_teleop - - bitbots_tf_buffer - bitbots_tts - bitbots_utils - system_monitor @@ -51,6 +50,7 @@ include: - bio_ik - bio_ik_msgs - biped_interfaces + - bitbots_tf_buffer - dynamic_stack_decider - dynamixel-workbench - DynamixelSDK From 5147b608925ff8235145c6c9ab8a2a5dfc9bc599 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?P=C3=A4r=20Wiegmann?= Date: Wed, 22 Jan 2025 15:43:48 +0100 Subject: [PATCH 31/47] Fix typing issues in vision --- bitbots_vision/bitbots_vision/vision_modules/yoeo/utils.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/bitbots_vision/bitbots_vision/vision_modules/yoeo/utils.py b/bitbots_vision/bitbots_vision/vision_modules/yoeo/utils.py index 15a11301d..22c8ae311 100644 --- a/bitbots_vision/bitbots_vision/vision_modules/yoeo/utils.py +++ b/bitbots_vision/bitbots_vision/vision_modules/yoeo/utils.py @@ -5,7 +5,7 @@ import cv2 import numpy as np import rclpy -from jaxtyping import UInt8 +from jaxtyping import Float64, UInt8 logger = rclpy.logging.get_logger("yoeo_handler_utils") @@ -158,10 +158,10 @@ def get_info(self) -> ImagePreProcessorData: padding_bottom=self._padding_bottom, padding_left=self._padding_left, padding_right=self._padding_right, - max_dim=np.max(self._image_dimensions_HW), + max_dim=int(np.max(self._image_dimensions_HW)), ) - def process(self, image: UInt8[np.ndarray, "h w 3"]) -> UInt8[np.ndarray, "3 network_input_h network_input_w"]: + def process(self, image: UInt8[np.ndarray, "h w 3"]) -> Float64[np.ndarray, "3 network_input_h network_input_w"]: self._image_dimensions_HW = image.shape[:2] # type: ignore[assignment] self._calculate_paddings() From 547322f95dcf0bf7ee1abf91f6e2929a09bb6d93 Mon Sep 17 00:00:00 2001 From: Lea Date: Wed, 22 Jan 2025 20:14:40 +0100 Subject: [PATCH 32/47] removed measuring of planning step for evaluation --- .../bitbots_path_planning/path_planning.py | 8 -------- 1 file changed, 8 deletions(-) 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 76dd4adfe..010125a4b 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 @@ -90,15 +90,7 @@ def step(self) -> None: try: if self.planner.active(): # Calculate the path to the goal pose considering the current map - start = time() path = self.planner.step() - end = time() - if len(self.planner.robots) not in self.measurements: - self.measurements[len(self.planner.robots)] = (1, end - start) - else: - (n, avg) = self.measurements[len(self.planner.robots)] - self.measurements[len(self.planner.robots)] = (n + 1, (n * avg + (end - start)) / (n + 1)) - self.get_logger().info(f"planning steps={self.measurements}") # Publish the path for visualization self.path_pub.publish(path) # Calculate the command velocity to follow the given path From 3529a97e9884b1a743e563056cbed2f5b50d0f70 Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Thu, 27 Feb 2025 19:04:12 +0100 Subject: [PATCH 33/47] Rename rust nav package --- .../.github/workflows/CI.yml | 0 .../{bitbots_path_planning_rust => bitbots_rust_nav}/.gitignore | 0 .../{bitbots_path_planning_rust => bitbots_rust_nav}/Cargo.lock | 0 .../{bitbots_path_planning_rust => bitbots_rust_nav}/Cargo.toml | 0 .../{bitbots_path_planning_rust => bitbots_rust_nav}/README.md | 0 .../pyproject.toml | 0 .../{bitbots_path_planning_rust => bitbots_rust_nav}/src/lib.rs | 0 .../{bitbots_path_planning_rust => bitbots_rust_nav}/src/map.rs | 0 .../src/obstacle.rs | 0 .../src/planner.rs | 0 requirements/common.txt | 1 - 11 files changed, 1 deletion(-) rename bitbots_navigation/{bitbots_path_planning_rust => bitbots_rust_nav}/.github/workflows/CI.yml (100%) rename bitbots_navigation/{bitbots_path_planning_rust => bitbots_rust_nav}/.gitignore (100%) rename bitbots_navigation/{bitbots_path_planning_rust => bitbots_rust_nav}/Cargo.lock (100%) rename bitbots_navigation/{bitbots_path_planning_rust => bitbots_rust_nav}/Cargo.toml (100%) rename bitbots_navigation/{bitbots_path_planning_rust => bitbots_rust_nav}/README.md (100%) rename bitbots_navigation/{bitbots_path_planning_rust => bitbots_rust_nav}/pyproject.toml (100%) rename bitbots_navigation/{bitbots_path_planning_rust => bitbots_rust_nav}/src/lib.rs (100%) rename bitbots_navigation/{bitbots_path_planning_rust => bitbots_rust_nav}/src/map.rs (100%) rename bitbots_navigation/{bitbots_path_planning_rust => bitbots_rust_nav}/src/obstacle.rs (100%) rename bitbots_navigation/{bitbots_path_planning_rust => bitbots_rust_nav}/src/planner.rs (100%) diff --git a/bitbots_navigation/bitbots_path_planning_rust/.github/workflows/CI.yml b/bitbots_navigation/bitbots_rust_nav/.github/workflows/CI.yml similarity index 100% rename from bitbots_navigation/bitbots_path_planning_rust/.github/workflows/CI.yml rename to bitbots_navigation/bitbots_rust_nav/.github/workflows/CI.yml diff --git a/bitbots_navigation/bitbots_path_planning_rust/.gitignore b/bitbots_navigation/bitbots_rust_nav/.gitignore similarity index 100% rename from bitbots_navigation/bitbots_path_planning_rust/.gitignore rename to bitbots_navigation/bitbots_rust_nav/.gitignore diff --git a/bitbots_navigation/bitbots_path_planning_rust/Cargo.lock b/bitbots_navigation/bitbots_rust_nav/Cargo.lock similarity index 100% rename from bitbots_navigation/bitbots_path_planning_rust/Cargo.lock rename to bitbots_navigation/bitbots_rust_nav/Cargo.lock diff --git a/bitbots_navigation/bitbots_path_planning_rust/Cargo.toml b/bitbots_navigation/bitbots_rust_nav/Cargo.toml similarity index 100% rename from bitbots_navigation/bitbots_path_planning_rust/Cargo.toml rename to bitbots_navigation/bitbots_rust_nav/Cargo.toml diff --git a/bitbots_navigation/bitbots_path_planning_rust/README.md b/bitbots_navigation/bitbots_rust_nav/README.md similarity index 100% rename from bitbots_navigation/bitbots_path_planning_rust/README.md rename to bitbots_navigation/bitbots_rust_nav/README.md diff --git a/bitbots_navigation/bitbots_path_planning_rust/pyproject.toml b/bitbots_navigation/bitbots_rust_nav/pyproject.toml similarity index 100% rename from bitbots_navigation/bitbots_path_planning_rust/pyproject.toml rename to bitbots_navigation/bitbots_rust_nav/pyproject.toml diff --git a/bitbots_navigation/bitbots_path_planning_rust/src/lib.rs b/bitbots_navigation/bitbots_rust_nav/src/lib.rs similarity index 100% rename from bitbots_navigation/bitbots_path_planning_rust/src/lib.rs rename to bitbots_navigation/bitbots_rust_nav/src/lib.rs diff --git a/bitbots_navigation/bitbots_path_planning_rust/src/map.rs b/bitbots_navigation/bitbots_rust_nav/src/map.rs similarity index 100% rename from bitbots_navigation/bitbots_path_planning_rust/src/map.rs rename to bitbots_navigation/bitbots_rust_nav/src/map.rs diff --git a/bitbots_navigation/bitbots_path_planning_rust/src/obstacle.rs b/bitbots_navigation/bitbots_rust_nav/src/obstacle.rs similarity index 100% rename from bitbots_navigation/bitbots_path_planning_rust/src/obstacle.rs rename to bitbots_navigation/bitbots_rust_nav/src/obstacle.rs diff --git a/bitbots_navigation/bitbots_path_planning_rust/src/planner.rs b/bitbots_navigation/bitbots_rust_nav/src/planner.rs similarity index 100% rename from bitbots_navigation/bitbots_path_planning_rust/src/planner.rs rename to bitbots_navigation/bitbots_rust_nav/src/planner.rs diff --git a/requirements/common.txt b/requirements/common.txt index eeac6a55c..046b3efa4 100644 --- a/requirements/common.txt +++ b/requirements/common.txt @@ -3,7 +3,6 @@ pip transforms3d==0.4.1 git+https://github.com/Flova/pyastar2d git+https://github.com/bit-bots/YOEO -git+https://github.com/bit-bots/bitbots_pathplanning_rust simpleeval beartype jaxtyping From 847611a9339724d17b839b9d420f716f02cf6210 Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Thu, 27 Feb 2025 20:05:59 +0100 Subject: [PATCH 34/47] Remove the rust pathfinding code again --- .../bitbots_path_planning/planner.py | 2 +- .../bitbots_rust_nav/.github/workflows/CI.yml | 181 ------ .../bitbots_rust_nav/.gitignore | 72 --- .../bitbots_rust_nav/Cargo.lock | 545 ------------------ .../bitbots_rust_nav/Cargo.toml | 15 - bitbots_navigation/bitbots_rust_nav/README.md | 17 - .../bitbots_rust_nav/pyproject.toml | 15 - .../bitbots_rust_nav/src/lib.rs | 15 - .../bitbots_rust_nav/src/map.rs | 77 --- .../bitbots_rust_nav/src/obstacle.rs | 60 -- .../bitbots_rust_nav/src/planner.rs | 121 ---- requirements/common.txt | 1 + 12 files changed, 2 insertions(+), 1119 deletions(-) delete mode 100644 bitbots_navigation/bitbots_rust_nav/.github/workflows/CI.yml delete mode 100644 bitbots_navigation/bitbots_rust_nav/.gitignore delete mode 100644 bitbots_navigation/bitbots_rust_nav/Cargo.lock delete mode 100644 bitbots_navigation/bitbots_rust_nav/Cargo.toml delete mode 100644 bitbots_navigation/bitbots_rust_nav/README.md delete mode 100644 bitbots_navigation/bitbots_rust_nav/pyproject.toml delete mode 100644 bitbots_navigation/bitbots_rust_nav/src/lib.rs delete mode 100644 bitbots_navigation/bitbots_rust_nav/src/map.rs delete mode 100644 bitbots_navigation/bitbots_rust_nav/src/obstacle.rs delete mode 100644 bitbots_navigation/bitbots_rust_nav/src/planner.rs 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 2a5cf66a5..a987d3caf 100644 --- a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/planner.py +++ b/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/planner.py @@ -2,7 +2,7 @@ import soccer_vision_3d_msgs.msg as sv3dm import tf2_ros as tf2 -from bitbots_pathplanning_rust import ObstacleMap, ObstacleMapConfig, RoundObstacle +from bitbots_rust_nav import ObstacleMap, ObstacleMapConfig, RoundObstacle from geometry_msgs.msg import PoseStamped from nav_msgs.msg import Path from rclpy.duration import Duration diff --git a/bitbots_navigation/bitbots_rust_nav/.github/workflows/CI.yml b/bitbots_navigation/bitbots_rust_nav/.github/workflows/CI.yml deleted file mode 100644 index 01e101321..000000000 --- a/bitbots_navigation/bitbots_rust_nav/.github/workflows/CI.yml +++ /dev/null @@ -1,181 +0,0 @@ -# This file is autogenerated by maturin v1.7.5 -# To update, run -# -# maturin generate-ci github -# -name: CI - -on: - push: - branches: - - main - - master - tags: - - '*' - pull_request: - workflow_dispatch: - -permissions: - contents: read - -jobs: - linux: - runs-on: ${{ matrix.platform.runner }} - strategy: - matrix: - platform: - - runner: ubuntu-22.04 - target: x86_64 - - runner: ubuntu-22.04 - target: x86 - - runner: ubuntu-22.04 - target: aarch64 - - runner: ubuntu-22.04 - target: armv7 - - runner: ubuntu-22.04 - target: s390x - - runner: ubuntu-22.04 - target: ppc64le - steps: - - uses: actions/checkout@v4 - - uses: actions/setup-python@v5 - with: - python-version: 3.x - - name: Build wheels - uses: PyO3/maturin-action@v1 - with: - target: ${{ matrix.platform.target }} - args: --release --out dist --find-interpreter - sccache: 'true' - manylinux: auto - - name: Upload wheels - uses: actions/upload-artifact@v4 - with: - name: wheels-linux-${{ matrix.platform.target }} - path: dist - - musllinux: - runs-on: ${{ matrix.platform.runner }} - strategy: - matrix: - platform: - - runner: ubuntu-22.04 - target: x86_64 - - runner: ubuntu-22.04 - target: x86 - - runner: ubuntu-22.04 - target: aarch64 - - runner: ubuntu-22.04 - target: armv7 - steps: - - uses: actions/checkout@v4 - - uses: actions/setup-python@v5 - with: - python-version: 3.x - - name: Build wheels - uses: PyO3/maturin-action@v1 - with: - target: ${{ matrix.platform.target }} - args: --release --out dist --find-interpreter - sccache: 'true' - manylinux: musllinux_1_2 - - name: Upload wheels - uses: actions/upload-artifact@v4 - with: - name: wheels-musllinux-${{ matrix.platform.target }} - path: dist - - windows: - runs-on: ${{ matrix.platform.runner }} - strategy: - matrix: - platform: - - runner: windows-latest - target: x64 - - runner: windows-latest - target: x86 - steps: - - uses: actions/checkout@v4 - - uses: actions/setup-python@v5 - with: - python-version: 3.x - architecture: ${{ matrix.platform.target }} - - name: Build wheels - uses: PyO3/maturin-action@v1 - with: - target: ${{ matrix.platform.target }} - args: --release --out dist --find-interpreter - sccache: 'true' - - name: Upload wheels - uses: actions/upload-artifact@v4 - with: - name: wheels-windows-${{ matrix.platform.target }} - path: dist - - macos: - runs-on: ${{ matrix.platform.runner }} - strategy: - matrix: - platform: - - runner: macos-13 - target: x86_64 - - runner: macos-14 - target: aarch64 - steps: - - uses: actions/checkout@v4 - - uses: actions/setup-python@v5 - with: - python-version: 3.x - - name: Build wheels - uses: PyO3/maturin-action@v1 - with: - target: ${{ matrix.platform.target }} - args: --release --out dist --find-interpreter - sccache: 'true' - - name: Upload wheels - uses: actions/upload-artifact@v4 - with: - name: wheels-macos-${{ matrix.platform.target }} - path: dist - - sdist: - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v4 - - name: Build sdist - uses: PyO3/maturin-action@v1 - with: - command: sdist - args: --out dist - - name: Upload sdist - uses: actions/upload-artifact@v4 - with: - name: wheels-sdist - path: dist - - release: - name: Release - runs-on: ubuntu-latest - if: ${{ startsWith(github.ref, 'refs/tags/') || github.event_name == 'workflow_dispatch' }} - needs: [linux, musllinux, windows, macos, sdist] - permissions: - # Use to sign the release artifacts - id-token: write - # Used to upload release artifacts - contents: write - # Used to generate artifact attestation - attestations: write - steps: - - uses: actions/download-artifact@v4 - - name: Generate artifact attestation - uses: actions/attest-build-provenance@v1 - with: - subject-path: 'wheels-*/*' - - name: Publish to PyPI - if: "startsWith(github.ref, 'refs/tags/')" - uses: PyO3/maturin-action@v1 - env: - MATURIN_PYPI_TOKEN: ${{ secrets.PYPI_API_TOKEN }} - with: - command: upload - args: --non-interactive --skip-existing wheels-*/* diff --git a/bitbots_navigation/bitbots_rust_nav/.gitignore b/bitbots_navigation/bitbots_rust_nav/.gitignore deleted file mode 100644 index c8f044299..000000000 --- a/bitbots_navigation/bitbots_rust_nav/.gitignore +++ /dev/null @@ -1,72 +0,0 @@ -/target - -# Byte-compiled / optimized / DLL files -__pycache__/ -.pytest_cache/ -*.py[cod] - -# C extensions -*.so - -# Distribution / packaging -.Python -.venv/ -env/ -bin/ -build/ -develop-eggs/ -dist/ -eggs/ -lib/ -lib64/ -parts/ -sdist/ -var/ -include/ -man/ -venv/ -*.egg-info/ -.installed.cfg -*.egg - -# Installer logs -pip-log.txt -pip-delete-this-directory.txt -pip-selfcheck.json - -# Unit test / coverage reports -htmlcov/ -.tox/ -.coverage -.cache -nosetests.xml -coverage.xml - -# Translations -*.mo - -# Mr Developer -.mr.developer.cfg -.project -.pydevproject - -# Rope -.ropeproject - -# Django stuff: -*.log -*.pot - -.DS_Store - -# Sphinx documentation -docs/_build/ - -# PyCharm -.idea/ - -# VSCode -.vscode/ - -# Pyenv -.python-version diff --git a/bitbots_navigation/bitbots_rust_nav/Cargo.lock b/bitbots_navigation/bitbots_rust_nav/Cargo.lock deleted file mode 100644 index 2203a6978..000000000 --- a/bitbots_navigation/bitbots_rust_nav/Cargo.lock +++ /dev/null @@ -1,545 +0,0 @@ -# This file is automatically @generated by Cargo. -# It is not intended for manual editing. -version = 3 - -[[package]] -name = "ahash" -version = "0.8.11" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e89da841a80418a9b391ebaea17f5c112ffaaa96f621d2c285b5174da76b9011" -dependencies = [ - "cfg-if", - "once_cell", - "version_check", - "zerocopy", -] - -[[package]] -name = "allocator-api2" -version = "0.2.21" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "683d7910e743518b0e34f1186f92494becacb047c7b6bf616c96772180fef923" - -[[package]] -name = "approx" -version = "0.5.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "cab112f0a86d568ea0e627cc1d6be74a1e9cd55214684db5561995f6dad897c6" -dependencies = [ - "num-traits", -] - -[[package]] -name = "autocfg" -version = "1.4.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ace50bade8e6234aa140d9a2f552bbee1db4d353f69b8217bc503490fc1a9f26" - -[[package]] -name = "bitbots_pathplanning_rust" -version = "0.1.2" -dependencies = [ - "geo", - "keyed_priority_queue", - "ordered-float", - "pyo3", -] - -[[package]] -name = "byteorder" -version = "1.5.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "1fd0f2584146f6f2ef48085050886acf353beff7305ebd1ae69500e27c67f64b" - -[[package]] -name = "cfg-if" -version = "1.0.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "baf1de4339761588bc0619e3cbc0120ee582ebb74b53b4efbf79117bd2da40fd" - -[[package]] -name = "crossbeam-deque" -version = "0.8.6" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9dd111b7b7f7d55b72c0a6ae361660ee5853c9af73f70c3c2ef6858b950e2e51" -dependencies = [ - "crossbeam-epoch", - "crossbeam-utils", -] - -[[package]] -name = "crossbeam-epoch" -version = "0.9.18" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5b82ac4a3c2ca9c3460964f020e1402edd5753411d7737aa39c3714ad1b5420e" -dependencies = [ - "crossbeam-utils", -] - -[[package]] -name = "crossbeam-utils" -version = "0.8.21" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d0a5c400df2834b80a4c3327b3aad3a4c4cd4de0629063962b03235697506a28" - -[[package]] -name = "earcutr" -version = "0.4.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "79127ed59a85d7687c409e9978547cffb7dc79675355ed22da6b66fd5f6ead01" -dependencies = [ - "itertools", - "num-traits", -] - -[[package]] -name = "either" -version = "1.13.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "60b1af1c220855b6ceac025d3f6ecdd2b7c4894bfe9cd9bda4fbb4bc7c0d4cf0" - -[[package]] -name = "equivalent" -version = "1.0.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5443807d6dff69373d433ab9ef5378ad8df50ca6298caf15de6e52e24aaf54d5" - -[[package]] -name = "float_next_after" -version = "1.0.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8bf7cc16383c4b8d58b9905a8509f02926ce3058053c056376248d958c9df1e8" - -[[package]] -name = "geo" -version = "0.29.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "34f0e6e028c581e82e6822a68869514e94c25e7f8ea669a2d8595bdf7461ccc5" -dependencies = [ - "earcutr", - "float_next_after", - "geo-types", - "geographiclib-rs", - "i_overlay", - "log", - "num-traits", - "robust", - "rstar", - "spade", -] - -[[package]] -name = "geo-types" -version = "0.7.14" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b6f47c611187777bbca61ea7aba780213f5f3441fd36294ab333e96cfa791b65" -dependencies = [ - "approx", - "num-traits", - "rayon", - "rstar", - "serde", -] - -[[package]] -name = "geographiclib-rs" -version = "0.2.4" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e6e5ed84f8089c70234b0a8e0aedb6dc733671612ddc0d37c6066052f9781960" -dependencies = [ - "libm", -] - -[[package]] -name = "hash32" -version = "0.3.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "47d60b12902ba28e2730cd37e95b8c9223af2808df9e902d4df49588d1470606" -dependencies = [ - "byteorder", -] - -[[package]] -name = "hashbrown" -version = "0.14.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e5274423e17b7c9fc20b6e7e208532f9b19825d82dfd615708b70edd83df41f1" -dependencies = [ - "ahash", - "allocator-api2", -] - -[[package]] -name = "hashbrown" -version = "0.15.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "bf151400ff0baff5465007dd2f3e717f3fe502074ca563069ce3a6629d07b289" - -[[package]] -name = "heapless" -version = "0.8.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0bfb9eb618601c89945a70e254898da93b13be0388091d42117462b265bb3fad" -dependencies = [ - "hash32", - "stable_deref_trait", -] - -[[package]] -name = "heck" -version = "0.5.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "2304e00983f87ffb38b55b444b5e3b60a884b5d30c0fca7d82fe33449bbe55ea" - -[[package]] -name = "i_float" -version = "1.6.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "775f9961a8d2f879725da8aff789bb20a3ddf297473e0c90af75e69313919490" -dependencies = [ - "serde", -] - -[[package]] -name = "i_key_sort" -version = "0.2.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "347c253b4748a1a28baf94c9ce133b6b166f08573157e05afe718812bc599fcd" - -[[package]] -name = "i_overlay" -version = "1.9.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "06740cd31c1f963823e007d8e6edcd2db634b2856f4f613e3df01737fd852482" -dependencies = [ - "i_float", - "i_key_sort", - "i_shape", - "i_tree", - "rayon", -] - -[[package]] -name = "i_shape" -version = "1.6.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "27dbe9e5238d6b9c694c08415bf00fb370b089949bd818ab01f41f8927b8774c" -dependencies = [ - "i_float", - "serde", -] - -[[package]] -name = "i_tree" -version = "0.8.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "155181bc97d770181cf9477da51218a19ee92a8e5be642e796661aee2b601139" - -[[package]] -name = "indexmap" -version = "2.7.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "62f822373a4fe84d4bb149bf54e584a7f4abec90e072ed49cda0edea5b95471f" -dependencies = [ - "equivalent", - "hashbrown 0.15.2", -] - -[[package]] -name = "indoc" -version = "2.0.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b248f5224d1d606005e02c97f5aa4e88eeb230488bcc03bc9ca4d7991399f2b5" - -[[package]] -name = "itertools" -version = "0.11.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b1c173a5686ce8bfa551b3563d0c2170bf24ca44da99c7ca4bfdab5418c3fe57" -dependencies = [ - "either", -] - -[[package]] -name = "keyed_priority_queue" -version = "0.4.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "4ee7893dab2e44ae5f9d0173f26ff4aa327c10b01b06a72b52dd9405b628640d" -dependencies = [ - "indexmap", -] - -[[package]] -name = "libc" -version = "0.2.168" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5aaeb2981e0606ca11d79718f8bb01164f1d6ed75080182d3abf017e6d244b6d" - -[[package]] -name = "libm" -version = "0.2.11" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8355be11b20d696c8f18f6cc018c4e372165b1fa8126cef092399c9951984ffa" - -[[package]] -name = "log" -version = "0.4.22" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a7a70ba024b9dc04c27ea2f0c0548feb474ec5c54bba33a7f72f873a39d07b24" - -[[package]] -name = "memoffset" -version = "0.9.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "488016bfae457b036d996092f6cb448677611ce4449e970ceaf42695203f218a" -dependencies = [ - "autocfg", -] - -[[package]] -name = "num-traits" -version = "0.2.19" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "071dfc062690e90b734c0b2273ce72ad0ffa95f0c74596bc250dcfd960262841" -dependencies = [ - "autocfg", - "libm", -] - -[[package]] -name = "once_cell" -version = "1.20.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "1261fe7e33c73b354eab43b1273a57c8f967d0391e80353e51f764ac02cf6775" - -[[package]] -name = "ordered-float" -version = "4.5.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c65ee1f9701bf938026630b455d5315f490640234259037edb259798b3bcf85e" -dependencies = [ - "num-traits", -] - -[[package]] -name = "portable-atomic" -version = "1.10.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "280dc24453071f1b63954171985a0b0d30058d287960968b9b2aca264c8d4ee6" - -[[package]] -name = "proc-macro2" -version = "1.0.92" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "37d3544b3f2748c54e147655edb5025752e2303145b5aefb3c3ea2c78b973bb0" -dependencies = [ - "unicode-ident", -] - -[[package]] -name = "pyo3" -version = "0.23.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e484fd2c8b4cb67ab05a318f1fd6fa8f199fcc30819f08f07d200809dba26c15" -dependencies = [ - "cfg-if", - "indoc", - "libc", - "memoffset", - "once_cell", - "portable-atomic", - "pyo3-build-config", - "pyo3-ffi", - "pyo3-macros", - "unindent", -] - -[[package]] -name = "pyo3-build-config" -version = "0.23.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "dc0e0469a84f208e20044b98965e1561028180219e35352a2afaf2b942beff3b" -dependencies = [ - "once_cell", - "target-lexicon", -] - -[[package]] -name = "pyo3-ffi" -version = "0.23.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "eb1547a7f9966f6f1a0f0227564a9945fe36b90da5a93b3933fc3dc03fae372d" -dependencies = [ - "libc", - "pyo3-build-config", -] - -[[package]] -name = "pyo3-macros" -version = "0.23.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "fdb6da8ec6fa5cedd1626c886fc8749bdcbb09424a86461eb8cdf096b7c33257" -dependencies = [ - "proc-macro2", - "pyo3-macros-backend", - "quote", - "syn", -] - -[[package]] -name = "pyo3-macros-backend" -version = "0.23.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "38a385202ff5a92791168b1136afae5059d3ac118457bb7bc304c197c2d33e7d" -dependencies = [ - "heck", - "proc-macro2", - "pyo3-build-config", - "quote", - "syn", -] - -[[package]] -name = "quote" -version = "1.0.37" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b5b9d34b8991d19d98081b46eacdd8eb58c6f2b201139f7c5f643cc155a633af" -dependencies = [ - "proc-macro2", -] - -[[package]] -name = "rayon" -version = "1.10.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b418a60154510ca1a002a752ca9714984e21e4241e804d32555251faf8b78ffa" -dependencies = [ - "either", - "rayon-core", -] - -[[package]] -name = "rayon-core" -version = "1.12.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "1465873a3dfdaa8ae7cb14b4383657caab0b3e8a0aa9ae8e04b044854c8dfce2" -dependencies = [ - "crossbeam-deque", - "crossbeam-utils", -] - -[[package]] -name = "robust" -version = "1.1.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "cbf4a6aa5f6d6888f39e980649f3ad6b666acdce1d78e95b8a2cb076e687ae30" - -[[package]] -name = "rstar" -version = "0.12.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "421400d13ccfd26dfa5858199c30a5d76f9c54e0dba7575273025b43c5175dbb" -dependencies = [ - "heapless", - "num-traits", - "smallvec", -] - -[[package]] -name = "serde" -version = "1.0.216" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0b9781016e935a97e8beecf0c933758c97a5520d32930e460142b4cd80c6338e" -dependencies = [ - "serde_derive", -] - -[[package]] -name = "serde_derive" -version = "1.0.216" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "46f859dbbf73865c6627ed570e78961cd3ac92407a2d117204c49232485da55e" -dependencies = [ - "proc-macro2", - "quote", - "syn", -] - -[[package]] -name = "smallvec" -version = "1.13.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3c5e1a9a646d36c3599cd173a41282daf47c44583ad367b8e6837255952e5c67" - -[[package]] -name = "spade" -version = "2.12.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "93f5ef1f863aca7d1d7dda7ccfc36a0a4279bd6d3c375176e5e0712e25cb4889" -dependencies = [ - "hashbrown 0.14.5", - "num-traits", - "robust", - "smallvec", -] - -[[package]] -name = "stable_deref_trait" -version = "1.2.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a8f112729512f8e442d81f95a8a7ddf2b7c6b8a1a6f509a95864142b30cab2d3" - -[[package]] -name = "syn" -version = "2.0.90" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "919d3b74a5dd0ccd15aeb8f93e7006bd9e14c295087c9896a110f490752bcf31" -dependencies = [ - "proc-macro2", - "quote", - "unicode-ident", -] - -[[package]] -name = "target-lexicon" -version = "0.12.16" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "61c41af27dd6d1e27b1b16b489db798443478cef1f06a660c96db617ba5de3b1" - -[[package]] -name = "unicode-ident" -version = "1.0.14" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "adb9e6ca4f869e1180728b7950e35922a7fc6397f7b641499e8f3ef06e50dc83" - -[[package]] -name = "unindent" -version = "0.2.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c7de7d73e1754487cb58364ee906a499937a0dfabd86bcb980fa99ec8c8fa2ce" - -[[package]] -name = "version_check" -version = "0.9.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0b928f33d975fc6ad9f86c8f283853ad26bdd5b10b7f1542aa2fa15e2289105a" - -[[package]] -name = "zerocopy" -version = "0.7.35" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "1b9b4fd18abc82b8136838da5d50bae7bdea537c574d8dc1a34ed098d6c166f0" -dependencies = [ - "zerocopy-derive", -] - -[[package]] -name = "zerocopy-derive" -version = "0.7.35" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "fa4f8080344d4671fb4e831a13ad1e68092748387dfc4f55e356242fae12ce3e" -dependencies = [ - "proc-macro2", - "quote", - "syn", -] diff --git a/bitbots_navigation/bitbots_rust_nav/Cargo.toml b/bitbots_navigation/bitbots_rust_nav/Cargo.toml deleted file mode 100644 index 0fae859f3..000000000 --- a/bitbots_navigation/bitbots_rust_nav/Cargo.toml +++ /dev/null @@ -1,15 +0,0 @@ -[package] -name = "bitbots_pathplanning_rust" -version = "0.1.2" -edition = "2021" - -# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html -[lib] -name = "bitbots_pathplanning_rust" -crate-type = ["cdylib"] - -[dependencies] -geo = "0.29.3" -keyed_priority_queue = "0.4.2" -ordered-float = "4.5.0" -pyo3 = "0.23.1" diff --git a/bitbots_navigation/bitbots_rust_nav/README.md b/bitbots_navigation/bitbots_rust_nav/README.md deleted file mode 100644 index ccbf0abb8..000000000 --- a/bitbots_navigation/bitbots_rust_nav/README.md +++ /dev/null @@ -1,17 +0,0 @@ -# bitbots_pathplanning_rust - -A best-effort A*-on-visibility-graph implementation for pathplanning in obstacles maps where obstacles and the robot itself are assumed to be round. - -It exports three classes - -## `RoundObstacle` - -Represents a round obstacle with public fields `center: (float, float)` and `radius: float`. Can be created with the constructor `RoundObstacle(center, radius)` - -## `ObstacleMapConfig` - -Represents all configuration values for how obstacles should be treated with public fields `dilate: float` - the value by which the radii of the obstacles should be dilated (this should be set to approximately your own radius) - and `num_vertices` - the number of vertices the polygons approximating the round obstacles should have. Can be created with the constructor `ObstacleMapConfig(dilate, num_vertices)` - -## `ObstacleMap` - -Represents a set of obstacles on a plane with a given config. This has two fields: `config: ObstacleMapConfig` and `obstacles: [RoundObstacle]`. Can be created with the constructor `ObstacleMap(config, obstacles)` \ No newline at end of file diff --git a/bitbots_navigation/bitbots_rust_nav/pyproject.toml b/bitbots_navigation/bitbots_rust_nav/pyproject.toml deleted file mode 100644 index 14318d892..000000000 --- a/bitbots_navigation/bitbots_rust_nav/pyproject.toml +++ /dev/null @@ -1,15 +0,0 @@ -[build-system] -requires = ["maturin>=1.7,<2.0"] -build-backend = "maturin" - -[project] -name = "bitbots_pathplanning_rust" -requires-python = ">=3.8" -classifiers = [ - "Programming Language :: Rust", - "Programming Language :: Python :: Implementation :: CPython", - "Programming Language :: Python :: Implementation :: PyPy", -] -dynamic = ["version"] -[tool.maturin] -features = ["pyo3/extension-module"] diff --git a/bitbots_navigation/bitbots_rust_nav/src/lib.rs b/bitbots_navigation/bitbots_rust_nav/src/lib.rs deleted file mode 100644 index 0cddab3de..000000000 --- a/bitbots_navigation/bitbots_rust_nav/src/lib.rs +++ /dev/null @@ -1,15 +0,0 @@ -use map::{ObstacleMap, ObstacleMapConfig}; -use obstacle::RoundObstacle; -use pyo3::prelude::*; - -mod map; -mod obstacle; -mod planner; - -#[pymodule] -fn bitbots_pathplanning_rust(m: &Bound<'_, PyModule>) -> PyResult<()> { - m.add_class::()?; - m.add_class::()?; - m.add_class::()?; - Ok(()) -} diff --git a/bitbots_navigation/bitbots_rust_nav/src/map.rs b/bitbots_navigation/bitbots_rust_nav/src/map.rs deleted file mode 100644 index 66306a425..000000000 --- a/bitbots_navigation/bitbots_rust_nav/src/map.rs +++ /dev/null @@ -1,77 +0,0 @@ -use geo::{BooleanOps, Coord, LineString, MultiPolygon, Polygon}; -use pyo3::prelude::*; - -use crate::{obstacle::{Obstacle, RoundObstacle}, planner::PathPlanner}; - -/// Configuration values for the ObstacleMap, these should be given by ROS parameters -#[pyclass( - eq, - str = "ObstacleMapConfig(dilate={dilate:?} num_vertices={num_vertices:?})" -)] -#[derive(Debug, Copy, Clone, PartialEq)] -pub struct ObstacleMapConfig { - /// A distance by which the obstacles should be dilated - #[pyo3(set, get)] - pub dilate: f64, - /// The number of vertices that the polygon representing an obstacle has - #[pyo3(set, get)] - pub num_vertices: usize, -} - -#[pymethods] -impl ObstacleMapConfig { - #[new] - /// Create a new config with the given values - pub fn new(dilate: f64, num_vertices: usize) -> Self { - Self { - dilate, - num_vertices, - } - } -} - -#[pyclass(eq, str = "ObstacleMap(obstacles={obstacles:?})")] -#[derive(Debug, Clone, PartialEq)] -pub struct ObstacleMap { - #[pyo3(get, set)] - config: ObstacleMapConfig, - #[pyo3(get, set)] - obstacles: Vec, -} - -#[pymethods] -impl ObstacleMap { - #[new] - pub fn new(config: ObstacleMapConfig, obstacles: Vec) -> Self { - Self { config, obstacles } - } - - pub fn shortest_path(&self, start: (f64, f64), goal: (f64, f64)) -> Vec<(f64, f64)> { - PathPlanner::new(&self, start, goal).shortest_path() - } -} - -impl ObstacleMap { - pub fn as_vertices(&self) -> Vec { - self.obstacles - .iter() - .map(|obstacle| obstacle.as_vertices(&self.config)) - .flatten() - .collect() - } - - fn as_polygons(&self) -> Vec { - self.obstacles - .iter() - .map(|obstacle| obstacle.as_polygon(&self.config)) - .collect() - } - - pub fn as_multipolygon(&self) -> MultiPolygon { - let polygons = self.as_polygons(); - polygons.iter().fold( - MultiPolygon::new(vec![Polygon::new(LineString::new(vec![]), vec![])]), - |a, b| a.union(b), - ) - } -} diff --git a/bitbots_navigation/bitbots_rust_nav/src/obstacle.rs b/bitbots_navigation/bitbots_rust_nav/src/obstacle.rs deleted file mode 100644 index 1f50408d9..000000000 --- a/bitbots_navigation/bitbots_rust_nav/src/obstacle.rs +++ /dev/null @@ -1,60 +0,0 @@ -use pyo3::prelude::*; - -use geo::{Coord, LineString, Polygon}; - -use crate::map::ObstacleMapConfig; - -pub trait Obstacle { - /// Return the vertices of this obstacle for the visibility graph - fn as_vertices(&self, config: &ObstacleMapConfig) -> Vec; - - /// Return the polygon representation of this obstacle for intersection checking - fn as_polygon(&self, config: &ObstacleMapConfig) -> Polygon; -} - -/// A round obstacle -#[pyclass(eq, str = "RoundObstacle(center={center:?} radius={radius:?})")] -#[derive(Debug, Clone, Copy, PartialEq)] -pub struct RoundObstacle { - /// The center of the obstacle - #[pyo3(get, set)] - center: (f64, f64), - /// The radius of the obstacle - #[pyo3(get, set)] - radius: f64, -} - -#[pymethods] -impl RoundObstacle { - #[new] - pub fn new(center: (f64, f64), radius: f64) -> Self { - Self { center, radius } - } -} - -impl RoundObstacle { - /// A helper function to generate a num_vertices-sided Polygon with own radius plus offset - fn regular_ngon(&self, num_vertices: usize, offset: f64) -> Vec { - (0..num_vertices) - .map(|side| { - let angle = (side as f64) / (num_vertices as f64) * std::f64::consts::TAU; - let radius = self.radius + offset; - Coord::from(self.center) + Coord::from((radius * angle.cos(), radius * angle.sin())) - }) - .collect::>() - } -} - -/// Value subtracted from the radius of obstacles for intersection checking -pub const EPSILON: f64 = 0.01; - -impl Obstacle for RoundObstacle { - fn as_vertices(&self, config: &ObstacleMapConfig) -> Vec { - self.regular_ngon(config.num_vertices, config.dilate) - } - - fn as_polygon(&self, config: &ObstacleMapConfig) -> Polygon { - let vertices = self.regular_ngon(config.num_vertices, config.dilate - EPSILON); - Polygon::new(LineString::new(vertices), vec![]) - } -} diff --git a/bitbots_navigation/bitbots_rust_nav/src/planner.rs b/bitbots_navigation/bitbots_rust_nav/src/planner.rs deleted file mode 100644 index 18b5e46d1..000000000 --- a/bitbots_navigation/bitbots_rust_nav/src/planner.rs +++ /dev/null @@ -1,121 +0,0 @@ -use std::{cmp::Reverse, collections::{HashMap, HashSet}}; - -use geo::{Coord, Distance, Euclidean, Intersects, Line, MultiPolygon}; -use keyed_priority_queue::{Entry, KeyedPriorityQueue}; -use ordered_float::OrderedFloat; - -use crate::map::ObstacleMap; - -#[derive(Debug, Clone)] -pub struct PathPlanner { - vertices: Vec, - obstacle: MultiPolygon, - start: usize, - goal: usize, - open: KeyedPriorityQueue>>, - g_score: HashMap>, - successors: HashSet, - from: HashMap, -} - -const MULTIPLIER: f64 = 10.0; - - -impl PathPlanner { - pub fn new(map: &ObstacleMap, start: (f64, f64), goal: (f64, f64)) -> Self { - let mut vertices = map.as_vertices(); - vertices.append(&mut vec![start.into(), goal.into()]); - let obstacle = map.as_multipolygon(); - let start = vertices.len() - 2; - let goal = vertices.len() - 1; - let mut open = KeyedPriorityQueue::>>::new(); - open.push( - start, - Reverse(OrderedFloat(Euclidean::distance( - vertices[start], - vertices[goal], - ))), - ); - let from = HashMap::::new(); - let successors = HashSet::::from_iter(0..vertices.len()); - let mut g_score = HashMap::>::new(); - g_score.insert(start, OrderedFloat(0.0)); - Self { - vertices, - obstacle, - start, - goal, - open, - g_score, - successors, - from, - } - } - - fn heuristic(&self, node: usize) -> OrderedFloat { - self.distance(node, self.goal) - } - - fn distance(&self, from: usize, to: usize) -> OrderedFloat { - OrderedFloat(Euclidean::distance(self.vertices[from], self.vertices[to])) - } - - fn reconstruct_path(&self, vertex: usize) -> Vec<(f64, f64)> { - let mut path = vec![vertex]; - let mut current = vertex; - while let Some(previous) = self.from.get(¤t) { - current = *previous; - path.push(*previous); - } - path.reverse(); - return path - .into_iter() - .map(|idx| self.vertices[idx].x_y()) - .collect(); - } - - fn expand_node(&mut self, vertex: usize) { - let g_vertex = self - .g_score - .get(&vertex) - .unwrap_or(&OrderedFloat(std::f64::INFINITY)) - .clone(); - for successor in self.successors.iter().cloned() { - let multiplier = if self.obstacle.intersects(&Line::new(self.vertices[vertex], self.vertices[successor])) { - if self.goal == successor || self.start == vertex { - OrderedFloat(MULTIPLIER) - } else { - continue; - } - } else { - OrderedFloat(1.0) - }; - let g_successor = self.g_score.get(&successor).unwrap_or(&OrderedFloat(std::f64::INFINITY)).clone(); - let g_tentative = g_vertex + multiplier * self.distance(vertex, successor); - if g_tentative < g_successor { - self.from.insert(successor, vertex); - self.g_score.insert(successor, g_tentative); - let new_f_score = Reverse(g_tentative + self.heuristic(successor)); - match self.open.entry(successor) { - Entry::Occupied(entry) => { - entry.set_priority(new_f_score); - }, - Entry::Vacant(entry) => { - entry.set_priority(new_f_score); - }, - } - } - } - } - - pub fn shortest_path(mut self) -> Vec<(f64, f64)> { - while let Some((vertex, _)) = self.open.pop() { - if vertex == self.goal { - return self.reconstruct_path(vertex); - } - self.successors.remove(&vertex); - self.expand_node(vertex); - } - unreachable!() - } -} diff --git a/requirements/common.txt b/requirements/common.txt index 046b3efa4..6704edc84 100644 --- a/requirements/common.txt +++ b/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 +git+https://github.com/bit-bots/bitbots_rust_nav simpleeval beartype jaxtyping From 57dbe3a0fab6833308153047d7e45343fa1037eb Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Thu, 27 Feb 2025 20:08:35 +0100 Subject: [PATCH 35/47] Remove pytest stuff --- .gitignore | 2 + .../bitbots_path_planning/test/test.py | 52 ------------------- 2 files changed, 2 insertions(+), 52 deletions(-) delete mode 100644 bitbots_navigation/bitbots_path_planning/test/test.py diff --git a/.gitignore b/.gitignore index 1d019f1b1..3d75bbd0c 100644 --- a/.gitignore +++ b/.gitignore @@ -227,3 +227,5 @@ doku/* # Workspace git status file from the deploy tool **/workspace_status.json + +.pytest_cache/ diff --git a/bitbots_navigation/bitbots_path_planning/test/test.py b/bitbots_navigation/bitbots_path_planning/test/test.py deleted file mode 100644 index c8c87d6a5..000000000 --- a/bitbots_navigation/bitbots_path_planning/test/test.py +++ /dev/null @@ -1,52 +0,0 @@ -from random import uniform - -import matplotlib -import matplotlib.patches -from bbpprs import ObstacleMap, ObstacleMapConfig, Robot -from matplotlib import pyplot as plt - -config = ObstacleMapConfig(dilate=0.1) - - -def random_omap(config, obstacles): - l = [] - for _ in range(obstacles): - x = uniform(1.0, 9.0) - y = uniform(1.0, 9.0) - r = uniform(0.2, 2.0) - l.append(Robot((x, y), r)) - return ObstacleMap(config, l, None) - - -def debug_omap(config): - l = [] - l.append(Robot((1.0, 4.0), 1.0)) - l.append(Robot((4.2, 4.2), 1.0)) - l.append(Robot((5.0, 2.0), 1.4)) - l.append(Robot((10.0, 10.0), 0.5)) - return ObstacleMap(config, l, None) - - -""" -def random_omaps(config, n): - l = [] - for _ in range(n): - l.append(random_omap(config, 3)) - return l - -omaps = random_omaps(config, 100) - -_ = [om.shortest_path(start=(0.0, 0.0), goal=(10.0, 10.0)) for om in omaps] -""" - -omap = debug_omap(config) -path = omap.shortest_path(start=(0.0, 0.0), goal=(10.0, 10.0)) -print(path) -fig, ax = plt.subplots(1) -ax.axis([0, 11, 0, 11]) -for obstacle in omap.robots: - ax.add_patch(matplotlib.patches.RegularPolygon(obstacle.center, 12, radius=obstacle.radius)) -ax.plot([i[0] for i in path], [i[1] for i in path]) -print(str(omap.ball), [str(bot) for bot in omap.robots]) -# plt.plot(path) -plt.show() From 7c7873845f6407d2e4b87a39380e119ca9803685 Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Thu, 27 Feb 2025 20:09:31 +0100 Subject: [PATCH 36/47] Remove old auto gen file --- ...tbots_technical_challenge_vision_params.py | 502 ------------------ 1 file changed, 502 deletions(-) delete mode 100644 bitbots_misc/bitbots_technical_challenge_vision/bitbots_technical_challenge_vision/bitbots_technical_challenge_vision_params.py diff --git a/bitbots_misc/bitbots_technical_challenge_vision/bitbots_technical_challenge_vision/bitbots_technical_challenge_vision_params.py b/bitbots_misc/bitbots_technical_challenge_vision/bitbots_technical_challenge_vision/bitbots_technical_challenge_vision_params.py deleted file mode 100644 index d56253c9b..000000000 --- a/bitbots_misc/bitbots_technical_challenge_vision/bitbots_technical_challenge_vision/bitbots_technical_challenge_vision_params.py +++ /dev/null @@ -1,502 +0,0 @@ -# flake8: noqa - -# auto-generated DO NOT EDIT - -from rcl_interfaces.msg import ParameterDescriptor -from rcl_interfaces.msg import SetParametersResult -from rcl_interfaces.msg import FloatingPointRange, IntegerRange -from rclpy.clock import Clock -from rclpy.exceptions import InvalidParameterValueException -from rclpy.time import Time -import copy -import rclpy -import rclpy.parameter -from generate_parameter_library_py.python_validators import ParameterValidators - - -class bitbots_technical_challenge_vision: - class Params: - # for detecting if the parameter struct has been updated - stamp_ = Time() - - blue_lower_h = 92 - blue_upper_h = 110 - blue_lower_s = 90 - blue_upper_s = 236 - blue_lower_v = 0 - blue_upper_v = 255 - red_lower_h = 138 - red_upper_h = 179 - red_lower_s = 78 - red_upper_s = 255 - red_lower_v = 0 - red_upper_v = 255 - min_size = 20 - max_size = 400 - debug_mode = False - - class ParamListener: - def __init__(self, node, prefix=""): - self.prefix_ = prefix - self.params_ = bitbots_technical_challenge_vision.Params() - self.node_ = node - self.logger_ = rclpy.logging.get_logger("bitbots_technical_challenge_vision." + prefix) - - self.declare_params() - - self.node_.add_on_set_parameters_callback(self.update) - self.clock_ = Clock() - - def get_params(self): - tmp = self.params_.stamp_ - self.params_.stamp_ = None - paramCopy = copy.deepcopy(self.params_) - paramCopy.stamp_ = tmp - self.params_.stamp_ = tmp - return paramCopy - - def is_old(self, other_param): - return self.params_.stamp_ != other_param.stamp_ - - def unpack_parameter_dict(self, namespace: str, parameter_dict: dict): - """ - Flatten a parameter dictionary recursively. - - :param namespace: The namespace to prepend to the parameter names. - :param parameter_dict: A dictionary of parameters keyed by the parameter names - :return: A list of rclpy Parameter objects - """ - parameters = [] - for param_name, param_value in parameter_dict.items(): - full_param_name = namespace + param_name - # Unroll nested parameters - if isinstance(param_value, dict): - nested_params = self.unpack_parameter_dict( - namespace=full_param_name + rclpy.parameter.PARAMETER_SEPARATOR_STRING, - parameter_dict=param_value, - ) - parameters.extend(nested_params) - else: - parameters.append(rclpy.parameter.Parameter(full_param_name, value=param_value)) - return parameters - - def set_params_from_dict(self, param_dict): - params_to_set = self.unpack_parameter_dict("", param_dict) - self.update(params_to_set) - - def refresh_dynamic_parameters(self): - updated_params = self.get_params() - # TODO remove any destroyed dynamic parameters - - # declare any new dynamic parameters - - def update(self, parameters): - updated_params = self.get_params() - - for param in parameters: - if param.name == self.prefix_ + "blue_lower_h": - validation_result = ParameterValidators.bounds(param, 0, 179) - if validation_result: - return SetParametersResult(successful=False, reason=validation_result) - updated_params.blue_lower_h = param.value - self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) - - if param.name == self.prefix_ + "blue_upper_h": - validation_result = ParameterValidators.bounds(param, 0, 179) - if validation_result: - return SetParametersResult(successful=False, reason=validation_result) - updated_params.blue_upper_h = param.value - self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) - - if param.name == self.prefix_ + "blue_lower_s": - validation_result = ParameterValidators.bounds(param, 0, 255) - if validation_result: - return SetParametersResult(successful=False, reason=validation_result) - updated_params.blue_lower_s = param.value - self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) - - if param.name == self.prefix_ + "blue_upper_s": - validation_result = ParameterValidators.bounds(param, 0, 255) - if validation_result: - return SetParametersResult(successful=False, reason=validation_result) - updated_params.blue_upper_s = param.value - self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) - - if param.name == self.prefix_ + "blue_lower_v": - validation_result = ParameterValidators.bounds(param, 0, 255) - if validation_result: - return SetParametersResult(successful=False, reason=validation_result) - updated_params.blue_lower_v = param.value - self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) - - if param.name == self.prefix_ + "blue_upper_v": - validation_result = ParameterValidators.bounds(param, 0, 255) - if validation_result: - return SetParametersResult(successful=False, reason=validation_result) - updated_params.blue_upper_v = param.value - self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) - - if param.name == self.prefix_ + "red_lower_h": - validation_result = ParameterValidators.bounds(param, 0, 179) - if validation_result: - return SetParametersResult(successful=False, reason=validation_result) - updated_params.red_lower_h = param.value - self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) - - if param.name == self.prefix_ + "red_upper_h": - validation_result = ParameterValidators.bounds(param, 0, 179) - if validation_result: - return SetParametersResult(successful=False, reason=validation_result) - updated_params.red_upper_h = param.value - self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) - - if param.name == self.prefix_ + "red_lower_s": - validation_result = ParameterValidators.bounds(param, 0, 255) - if validation_result: - return SetParametersResult(successful=False, reason=validation_result) - updated_params.red_lower_s = param.value - self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) - - if param.name == self.prefix_ + "red_upper_s": - validation_result = ParameterValidators.bounds(param, 0, 255) - if validation_result: - return SetParametersResult(successful=False, reason=validation_result) - updated_params.red_upper_s = param.value - self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) - - if param.name == self.prefix_ + "red_lower_v": - validation_result = ParameterValidators.bounds(param, 0, 255) - if validation_result: - return SetParametersResult(successful=False, reason=validation_result) - updated_params.red_lower_v = param.value - self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) - - if param.name == self.prefix_ + "red_upper_v": - validation_result = ParameterValidators.bounds(param, 0, 255) - if validation_result: - return SetParametersResult(successful=False, reason=validation_result) - updated_params.red_upper_v = param.value - self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) - - if param.name == self.prefix_ + "min_size": - validation_result = ParameterValidators.gt_eq(param, 0) - if validation_result: - return SetParametersResult(successful=False, reason=validation_result) - updated_params.min_size = param.value - self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) - - if param.name == self.prefix_ + "max_size": - validation_result = ParameterValidators.gt_eq(param, 0) - if validation_result: - return SetParametersResult(successful=False, reason=validation_result) - updated_params.max_size = param.value - self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) - - if param.name == self.prefix_ + "debug_mode": - updated_params.debug_mode = param.value - self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) - - updated_params.stamp_ = self.clock_.now() - self.update_internal_params(updated_params) - return SetParametersResult(successful=True) - - def update_internal_params(self, updated_params): - self.params_ = updated_params - - def declare_params(self): - updated_params = self.get_params() - # declare all parameters and give default values to non-required ones - if not self.node_.has_parameter(self.prefix_ + "blue_lower_h"): - descriptor = ParameterDescriptor( - description="hue value of the lower boundary for blue obstacles", read_only=False - ) - descriptor.integer_range.append(IntegerRange()) - descriptor.integer_range[-1].from_value = 0 - descriptor.integer_range[-1].to_value = 179 - parameter = updated_params.blue_lower_h - self.node_.declare_parameter(self.prefix_ + "blue_lower_h", parameter, descriptor) - - if not self.node_.has_parameter(self.prefix_ + "blue_upper_h"): - descriptor = ParameterDescriptor( - description="hue value of the upper boundary for blue obstacles", read_only=False - ) - descriptor.integer_range.append(IntegerRange()) - descriptor.integer_range[-1].from_value = 0 - descriptor.integer_range[-1].to_value = 179 - parameter = updated_params.blue_upper_h - self.node_.declare_parameter(self.prefix_ + "blue_upper_h", parameter, descriptor) - - if not self.node_.has_parameter(self.prefix_ + "blue_lower_s"): - descriptor = ParameterDescriptor( - description="separation value of the lower boundary for blue obstacles", read_only=False - ) - descriptor.integer_range.append(IntegerRange()) - descriptor.integer_range[-1].from_value = 0 - descriptor.integer_range[-1].to_value = 255 - parameter = updated_params.blue_lower_s - self.node_.declare_parameter(self.prefix_ + "blue_lower_s", parameter, descriptor) - - if not self.node_.has_parameter(self.prefix_ + "blue_upper_s"): - descriptor = ParameterDescriptor( - description="separation value of the upper boundary for blue obstacles", read_only=False - ) - descriptor.integer_range.append(IntegerRange()) - descriptor.integer_range[-1].from_value = 0 - descriptor.integer_range[-1].to_value = 255 - parameter = updated_params.blue_upper_s - self.node_.declare_parameter(self.prefix_ + "blue_upper_s", parameter, descriptor) - - if not self.node_.has_parameter(self.prefix_ + "blue_lower_v"): - descriptor = ParameterDescriptor( - description="value value of the lower boundary for blue obstacles", read_only=False - ) - descriptor.integer_range.append(IntegerRange()) - descriptor.integer_range[-1].from_value = 0 - descriptor.integer_range[-1].to_value = 255 - parameter = updated_params.blue_lower_v - self.node_.declare_parameter(self.prefix_ + "blue_lower_v", parameter, descriptor) - - if not self.node_.has_parameter(self.prefix_ + "blue_upper_v"): - descriptor = ParameterDescriptor( - description="value value of the upper boundary for blue obstacles", read_only=False - ) - descriptor.integer_range.append(IntegerRange()) - descriptor.integer_range[-1].from_value = 0 - descriptor.integer_range[-1].to_value = 255 - parameter = updated_params.blue_upper_v - self.node_.declare_parameter(self.prefix_ + "blue_upper_v", parameter, descriptor) - - if not self.node_.has_parameter(self.prefix_ + "red_lower_h"): - descriptor = ParameterDescriptor( - description="hue value of the lower boundary for red obstacles", read_only=False - ) - descriptor.integer_range.append(IntegerRange()) - descriptor.integer_range[-1].from_value = 0 - descriptor.integer_range[-1].to_value = 179 - parameter = updated_params.red_lower_h - self.node_.declare_parameter(self.prefix_ + "red_lower_h", parameter, descriptor) - - if not self.node_.has_parameter(self.prefix_ + "red_upper_h"): - descriptor = ParameterDescriptor( - description="hue value of the upper boundary for red obstacles", read_only=False - ) - descriptor.integer_range.append(IntegerRange()) - descriptor.integer_range[-1].from_value = 0 - descriptor.integer_range[-1].to_value = 179 - parameter = updated_params.red_upper_h - self.node_.declare_parameter(self.prefix_ + "red_upper_h", parameter, descriptor) - - if not self.node_.has_parameter(self.prefix_ + "red_lower_s"): - descriptor = ParameterDescriptor( - description="separation value of the lower boundary for red obstacles", read_only=False - ) - descriptor.integer_range.append(IntegerRange()) - descriptor.integer_range[-1].from_value = 0 - descriptor.integer_range[-1].to_value = 255 - parameter = updated_params.red_lower_s - self.node_.declare_parameter(self.prefix_ + "red_lower_s", parameter, descriptor) - - if not self.node_.has_parameter(self.prefix_ + "red_upper_s"): - descriptor = ParameterDescriptor( - description="separation value of the upper boundary for red obstacles", read_only=False - ) - descriptor.integer_range.append(IntegerRange()) - descriptor.integer_range[-1].from_value = 0 - descriptor.integer_range[-1].to_value = 255 - parameter = updated_params.red_upper_s - self.node_.declare_parameter(self.prefix_ + "red_upper_s", parameter, descriptor) - - if not self.node_.has_parameter(self.prefix_ + "red_lower_v"): - descriptor = ParameterDescriptor( - description="value value of the lower boundary for red obstacles", read_only=False - ) - descriptor.integer_range.append(IntegerRange()) - descriptor.integer_range[-1].from_value = 0 - descriptor.integer_range[-1].to_value = 255 - parameter = updated_params.red_lower_v - self.node_.declare_parameter(self.prefix_ + "red_lower_v", parameter, descriptor) - - if not self.node_.has_parameter(self.prefix_ + "red_upper_v"): - descriptor = ParameterDescriptor( - description="value value of the upper boundary for red obstacles", read_only=False - ) - descriptor.integer_range.append(IntegerRange()) - descriptor.integer_range[-1].from_value = 0 - descriptor.integer_range[-1].to_value = 255 - parameter = updated_params.red_upper_v - self.node_.declare_parameter(self.prefix_ + "red_upper_v", parameter, descriptor) - - if not self.node_.has_parameter(self.prefix_ + "min_size"): - descriptor = ParameterDescriptor( - description="minimum size of an obstacle to be considered", read_only=False - ) - descriptor.integer_range.append(IntegerRange()) - descriptor.integer_range[-1].from_value = 0 - descriptor.integer_range[-1].to_value = 2**31 - 1 - parameter = updated_params.min_size - self.node_.declare_parameter(self.prefix_ + "min_size", parameter, descriptor) - - if not self.node_.has_parameter(self.prefix_ + "max_size"): - descriptor = ParameterDescriptor( - description="maximum size of an obstacle to be considered", read_only=False - ) - descriptor.integer_range.append(IntegerRange()) - descriptor.integer_range[-1].from_value = 0 - descriptor.integer_range[-1].to_value = 2**31 - 1 - parameter = updated_params.max_size - self.node_.declare_parameter(self.prefix_ + "max_size", parameter, descriptor) - - if not self.node_.has_parameter(self.prefix_ + "debug_mode"): - descriptor = ParameterDescriptor( - description="set true if debug images should be drawn and published", read_only=False - ) - parameter = updated_params.debug_mode - self.node_.declare_parameter(self.prefix_ + "debug_mode", parameter, descriptor) - - # TODO: need validation - # get parameters and fill struct fields - param = self.node_.get_parameter(self.prefix_ + "blue_lower_h") - self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) - validation_result = ParameterValidators.bounds(param, 0, 179) - if validation_result: - raise InvalidParameterValueException( - "blue_lower_h", - param.value, - "Invalid value set during initialization for parameter blue_lower_h: " + validation_result, - ) - updated_params.blue_lower_h = param.value - param = self.node_.get_parameter(self.prefix_ + "blue_upper_h") - self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) - validation_result = ParameterValidators.bounds(param, 0, 179) - if validation_result: - raise InvalidParameterValueException( - "blue_upper_h", - param.value, - "Invalid value set during initialization for parameter blue_upper_h: " + validation_result, - ) - updated_params.blue_upper_h = param.value - param = self.node_.get_parameter(self.prefix_ + "blue_lower_s") - self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) - validation_result = ParameterValidators.bounds(param, 0, 255) - if validation_result: - raise InvalidParameterValueException( - "blue_lower_s", - param.value, - "Invalid value set during initialization for parameter blue_lower_s: " + validation_result, - ) - updated_params.blue_lower_s = param.value - param = self.node_.get_parameter(self.prefix_ + "blue_upper_s") - self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) - validation_result = ParameterValidators.bounds(param, 0, 255) - if validation_result: - raise InvalidParameterValueException( - "blue_upper_s", - param.value, - "Invalid value set during initialization for parameter blue_upper_s: " + validation_result, - ) - updated_params.blue_upper_s = param.value - param = self.node_.get_parameter(self.prefix_ + "blue_lower_v") - self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) - validation_result = ParameterValidators.bounds(param, 0, 255) - if validation_result: - raise InvalidParameterValueException( - "blue_lower_v", - param.value, - "Invalid value set during initialization for parameter blue_lower_v: " + validation_result, - ) - updated_params.blue_lower_v = param.value - param = self.node_.get_parameter(self.prefix_ + "blue_upper_v") - self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) - validation_result = ParameterValidators.bounds(param, 0, 255) - if validation_result: - raise InvalidParameterValueException( - "blue_upper_v", - param.value, - "Invalid value set during initialization for parameter blue_upper_v: " + validation_result, - ) - updated_params.blue_upper_v = param.value - param = self.node_.get_parameter(self.prefix_ + "red_lower_h") - self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) - validation_result = ParameterValidators.bounds(param, 0, 179) - if validation_result: - raise InvalidParameterValueException( - "red_lower_h", - param.value, - "Invalid value set during initialization for parameter red_lower_h: " + validation_result, - ) - updated_params.red_lower_h = param.value - param = self.node_.get_parameter(self.prefix_ + "red_upper_h") - self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) - validation_result = ParameterValidators.bounds(param, 0, 179) - if validation_result: - raise InvalidParameterValueException( - "red_upper_h", - param.value, - "Invalid value set during initialization for parameter red_upper_h: " + validation_result, - ) - updated_params.red_upper_h = param.value - param = self.node_.get_parameter(self.prefix_ + "red_lower_s") - self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) - validation_result = ParameterValidators.bounds(param, 0, 255) - if validation_result: - raise InvalidParameterValueException( - "red_lower_s", - param.value, - "Invalid value set during initialization for parameter red_lower_s: " + validation_result, - ) - updated_params.red_lower_s = param.value - param = self.node_.get_parameter(self.prefix_ + "red_upper_s") - self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) - validation_result = ParameterValidators.bounds(param, 0, 255) - if validation_result: - raise InvalidParameterValueException( - "red_upper_s", - param.value, - "Invalid value set during initialization for parameter red_upper_s: " + validation_result, - ) - updated_params.red_upper_s = param.value - param = self.node_.get_parameter(self.prefix_ + "red_lower_v") - self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) - validation_result = ParameterValidators.bounds(param, 0, 255) - if validation_result: - raise InvalidParameterValueException( - "red_lower_v", - param.value, - "Invalid value set during initialization for parameter red_lower_v: " + validation_result, - ) - updated_params.red_lower_v = param.value - param = self.node_.get_parameter(self.prefix_ + "red_upper_v") - self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) - validation_result = ParameterValidators.bounds(param, 0, 255) - if validation_result: - raise InvalidParameterValueException( - "red_upper_v", - param.value, - "Invalid value set during initialization for parameter red_upper_v: " + validation_result, - ) - updated_params.red_upper_v = param.value - param = self.node_.get_parameter(self.prefix_ + "min_size") - self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) - validation_result = ParameterValidators.gt_eq(param, 0) - if validation_result: - raise InvalidParameterValueException( - "min_size", - param.value, - "Invalid value set during initialization for parameter min_size: " + validation_result, - ) - updated_params.min_size = param.value - param = self.node_.get_parameter(self.prefix_ + "max_size") - self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) - validation_result = ParameterValidators.gt_eq(param, 0) - if validation_result: - raise InvalidParameterValueException( - "max_size", - param.value, - "Invalid value set during initialization for parameter max_size: " + validation_result, - ) - updated_params.max_size = param.value - param = self.node_.get_parameter(self.prefix_ + "debug_mode") - self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) - updated_params.debug_mode = param.value - - self.update_internal_params(updated_params) From f521fbfe19d543dbae3d9ceb68b927c693a416f8 Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Thu, 27 Feb 2025 20:23:32 +0100 Subject: [PATCH 37/47] Add docs --- .../bitbots_path_planning/path_planning.py | 3 -- .../bitbots_path_planning/planner.py | 34 ++++++++++++++----- 2 files changed, 25 insertions(+), 12 deletions(-) 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 010125a4b..058488e58 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 @@ -1,6 +1,3 @@ -import time -from time import time - import rclpy import soccer_vision_3d_msgs.msg as sv3dm from bitbots_tf_buffer import Buffer 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 a987d3caf..53f4ff4e1 100644 --- a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/planner.py +++ b/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/planner.py @@ -1,4 +1,5 @@ from abc import ABC, abstractmethod +from typing import Optional import soccer_vision_3d_msgs.msg as sv3dm import tf2_ros as tf2 @@ -48,9 +49,9 @@ class VisibilityPlanner(Planner): def __init__(self, node: NodeWithConfig, buffer: tf2.BufferInterface) -> None: self.node = node self.buffer = buffer - self.robots = [] - self.ball = None - self.goal: PoseStamped | None = None + self.robots: list[RoundObstacle] = [] + self.ball: Optional[RoundObstacle] = None + self.goal: Optional[PoseStamped] = None self.base_footprint_frame: str = self.node.config.base_footprint_frame self.ball_obstacle_active: bool = True self.frame: str = self.node.config.map.planning_frame @@ -61,9 +62,12 @@ def set_robots(self, robots: sv3dm.RobotArray): point = PointStamped() point.header.frame_id = robots.header.frame_id point.point = robot.bb.center.position + # Use the maximum dimension of the bounding box as the radius radius = max(numpify(robot.bb.size)[:2]) / 2 try: + # Transform the point to the planning frame position = self.buffer.transform(point, self.frame).point + # Add the robot to the buffer if the transformation was successful new_buffer.append(RoundObstacle(center=(position.x, position.y), radius=radius)) except (tf2.ConnectivityException, tf2.LookupException, tf2.ExtrapolationException) as e: self.node.get_logger().warn(str(e)) @@ -74,7 +78,9 @@ def set_ball(self, ball: PoseWithCovarianceStamped) -> None: point.header.frame_id = ball.header.frame_id point.point = ball.pose.pose.position try: + # Transform the point to the planning frame tf_point = self.buffer.transform(point, self.frame).point + # Create a new ball obstacle self.ball = RoundObstacle(center=(tf_point.x, tf_point.y), radius=self.node.config.map.ball_diameter / 2.0) except (tf2.ConnectivityException, tf2.LookupException, tf2.ExtrapolationException) as e: self.ball = None @@ -107,28 +113,38 @@ def active(self) -> bool: return self.goal is not None def step(self) -> Path: + """ + Computes the next path to the goal + """ + # Define goal goal = (self.goal.pose.position.x, self.goal.pose.position.y) + # Get our current position my_position = self.buffer.lookup_transform( self.frame, self.base_footprint_frame, Time(), Duration(seconds=0.2) ).transform.translation start = (my_position.x, my_position.y) + + # Configure how obstacles are represented config = ObstacleMapConfig(dilate=self.node.config.map.inflation.dilate, num_vertices=12) + # Add robots to obstacles obstacles = self.robots.copy() + # Add ball to obstacles if active if self.ball is not None: obstacles.append(self.ball) - omap = ObstacleMap(config, obstacles) - path = omap.shortest_path(start, goal) + obstacle_map = ObstacleMap(config, obstacles) + + # Calculate the shortest path + path = obstacle_map.shortest_path(start, goal) + # Convert the path to a ROS messages def map_to_pose(position): pose = PoseStamped() pose.pose.position.x = position[0] pose.pose.position.y = position[1] return pose - - poses = list(map(map_to_pose, path)) - poses.append(self.goal) + # Generate the path message return Path( header=Header(frame_id=self.frame, stamp=self.node.get_clock().now().to_msg()), - poses=poses, + poses=list(map(map_to_pose, path)) + [self.goal], ) From 2ab88c5122de86c6ff0c1f128203e34f1631d7a6 Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Mon, 3 Mar 2025 11:38:39 +0100 Subject: [PATCH 38/47] Increase path planning gains --- .../config/path_planning_parameters.yaml | 12 +++--------- 1 file changed, 3 insertions(+), 9 deletions(-) diff --git a/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml b/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml index 8fd02d0b2..158a14530 100644 --- a/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml +++ b/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml @@ -69,12 +69,6 @@ bitbots_path_planning: type: double default_value: 0.3 description: 'The dilation value for the inflation' - read_only: true - blur: - type: int - default_value: 11 - description: 'The blur value for the inflation' - read_only: true controller: carrot_distance: @@ -85,7 +79,7 @@ bitbots_path_planning: bounds<>: [0, 100] max_rotation_vel: type: double - default_value: 0.3 + default_value: 0.6 description: 'The maximum rotation velocity of the robot in rad/s around the z-axis' validation: bounds<>: [0.0, 1.0] @@ -121,7 +115,7 @@ bitbots_path_planning: bounds<>: [0.0, 1.0] rotation_slow_down_factor: type: double - default_value: 0.3 + default_value: 0.6 description: 'Clamped p gain of the rotation controller' validation: bounds<>: [0.0, 1.0] @@ -133,7 +127,7 @@ bitbots_path_planning: bounds<>: [0.0, 1.0] orient_to_goal_distance: type: double - default_value: 1.0 + default_value: 0.25 description: 'Distance at which we switch from orienting towards the path to orienting towards the goal poses orientation (in meters)' validation: bounds<>: [0.0, 10.0] From 6c160d419223c0880e80550901997c01bb116a1a Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Tue, 4 Mar 2025 15:13:11 +0100 Subject: [PATCH 39/47] Remove old path planning dep --- requirements/common.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/requirements/common.txt b/requirements/common.txt index 6704edc84..e48c44278 100644 --- a/requirements/common.txt +++ b/requirements/common.txt @@ -1,7 +1,6 @@ # This file includes common dependencies for all environments pip transforms3d==0.4.1 -git+https://github.com/Flova/pyastar2d git+https://github.com/bit-bots/YOEO git+https://github.com/bit-bots/bitbots_rust_nav simpleeval From 6c46ec15b409fa75397d00a0f50b5699d3c121ed Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Tue, 4 Mar 2025 15:13:36 +0100 Subject: [PATCH 40/47] Remove unused parameters --- .../config/path_planning_parameters.yaml | 37 ++++--------------- .../launch/path_planning.launch | 2 - 2 files changed, 7 insertions(+), 32 deletions(-) diff --git a/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml b/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml index 158a14530..a1ec4dacf 100644 --- a/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml +++ b/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml @@ -16,34 +16,12 @@ bitbots_path_planning: description: 'The rate at which the path planning is executed' validation: bounds<>: [0.0, 100.0] - - planner: - dummy: - type: bool - default_value: false - description: 'If true, the dummy planner is used, which just returns a straight line to the goal. This ignores all obstacles, but also has no limitations on the map size.' - + map: planning_frame: type: string default_value: map description: 'The frame in which the path planning is done' - resolution: - type: int - default_value: 20 - description: 'Pixel per meter' - read_only: true - size: - x: - type: double - default_value: 11.0 - description: 'The size of the map in x direction' - read_only: true - y: - type: double - default_value: 8.0 - description: 'The size of the map in y direction' - read_only: true ball_update_topic: type: string default_value: ball_position_relative_filtered @@ -59,16 +37,15 @@ bitbots_path_planning: default_value: 0.13 description: 'The diameter of the ball' read_only: true - obstacle_value: - type: int - default_value: 50 - description: 'The value of the obstacles in the map' - read_only: true inflation: - dilate: + robot_radius: type: double default_value: 0.3 - description: 'The dilation value for the inflation' + description: 'Radius of a circle on the ground that represents the space occupied by our robot. Instead of planning with both a robot polygon/circle and an obstacle polygon, we just inflate the obstacles and assume the robot is a point. This is faster and simpler.' + obstacle_margin: + type: double + default_value: 0.1 + description: "Distance we want to keep to obstacles when planning a path around them. No immediate action is required if the robot is closer than this distance to an obstacle, but we don't consider paths this close during the visibility graph generation." controller: carrot_distance: diff --git a/bitbots_navigation/bitbots_path_planning/launch/path_planning.launch b/bitbots_navigation/bitbots_path_planning/launch/path_planning.launch index d87afb55c..0774f196a 100755 --- a/bitbots_navigation/bitbots_path_planning/launch/path_planning.launch +++ b/bitbots_navigation/bitbots_path_planning/launch/path_planning.launch @@ -1,9 +1,7 @@ - - From 23514f7a701c394f16edafe141888d15466dfd22 Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Tue, 4 Mar 2025 15:14:47 +0100 Subject: [PATCH 41/47] Remove unnesesary dummy parameter --- bitbots_misc/bitbots_bringup/launch/demo.launch | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/bitbots_misc/bitbots_bringup/launch/demo.launch b/bitbots_misc/bitbots_bringup/launch/demo.launch index 6076d5490..e3a627dba 100644 --- a/bitbots_misc/bitbots_bringup/launch/demo.launch +++ b/bitbots_misc/bitbots_bringup/launch/demo.launch @@ -10,13 +10,6 @@ - - - - - - - + From 6907a39796aeffe27fd6c16ef3cdd43622143313 Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Tue, 4 Mar 2025 15:35:03 +0100 Subject: [PATCH 42/47] make verbose CI debug build --- Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Makefile b/Makefile index 375fe05b5..0f1f43cf2 100644 --- a/Makefile +++ b/Makefile @@ -17,7 +17,7 @@ install-no-root: pull-init update-no-root pip: # Install and upgrade pip dependencies - pip install --upgrade -r requirements/dev.txt --user + pip install --upgrade -r requirements/dev.txt --user -v pre-commit: # Install pre-commit hooks for all submodules that have a .pre-commit-config.yaml file From af3eebf31c5f4db9644d638319594aaa8ca557d2 Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Tue, 4 Mar 2025 19:17:06 +0100 Subject: [PATCH 43/47] Fix positioning error, that leads to the robot getting stuck before the kick --- .../capsules/pathfinding_capsule.py | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/pathfinding_capsule.py b/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/pathfinding_capsule.py index 30754da64..8f9eb2d92 100644 --- a/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/pathfinding_capsule.py +++ b/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/pathfinding_capsule.py @@ -166,8 +166,17 @@ def get_ball_goal(self, target: BallGoalType, distance: float, side_offset: floa elif ball_x > self._blackboard.world_model.field_length / 2 - 0.2: goal_angle = math.pi + np.copysign(math.pi / 4, ball_y) - goal_x = ball_x - math.cos(goal_angle) * distance + math.sin(goal_angle) * side_offset - goal_y = ball_y - math.sin(goal_angle) * distance + math.cos(goal_angle) * side_offset + # We don't want to walk into the ball, so we add an offset to stop before the ball + approach_offset_x = math.cos(goal_angle) * distance + approach_offset_y = math.sin(goal_angle) * distance + + # We also want to kick the ball with one foot instead of the center between the feet + side_offset_x = math.cos(goal_angle - math.pi / 2) * side_offset + side_offset_y = math.sin(goal_angle - math.pi / 2) * side_offset + + # Calculate the goal position (has nothing to do with the soccer goal) + goal_x = ball_x - approach_offset_x + side_offset_x + goal_y = ball_y - approach_offset_y + side_offset_y ball_point = (goal_x, goal_y, goal_angle, self._blackboard.map_frame) From 3dc8353a206843fd29e4ddf3f7910ee6a80e0252 Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Tue, 4 Mar 2025 19:17:25 +0100 Subject: [PATCH 44/47] Fix approach marker --- .../bitbots_body_behavior/behavior_dsd/actions/go_to_ball.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/go_to_ball.py b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/go_to_ball.py index ebb24ec83..728a56811 100644 --- a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/go_to_ball.py +++ b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/go_to_ball.py @@ -2,7 +2,6 @@ from bitbots_blackboard.capsules.pathfinding_capsule import BallGoalType from dynamic_stack_decider.abstract_action_element import AbstractActionElement from geometry_msgs.msg import Vector3 -from rclpy.duration import Duration from std_msgs.msg import ColorRGBA from visualization_msgs.msg import Marker @@ -31,6 +30,7 @@ def perform(self, reevaluate=False): approach_marker = Marker() approach_marker.pose.position.x = self.distance + approach_marker.pose.position.y = self.side_offset approach_marker.type = Marker.SPHERE approach_marker.action = Marker.MODIFY approach_marker.id = 1 @@ -40,7 +40,6 @@ def perform(self, reevaluate=False): color.b = 1.0 color.a = 1.0 approach_marker.color = color - approach_marker.lifetime = Duration(seconds=0.5).to_msg() scale = Vector3(x=0.2, y=0.2, z=0.2) approach_marker.scale = scale approach_marker.header.stamp = self.blackboard.node.get_clock().now().to_msg() From 4343f8ef44f1ae9f3ec550909a3dd2acd8e9c179 Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Tue, 4 Mar 2025 19:18:15 +0100 Subject: [PATCH 45/47] Fix race condition in path_planning --- .../bitbots_path_planning/path_planning.py | 43 +++---------------- 1 file changed, 6 insertions(+), 37 deletions(-) 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 058488e58..c69bf584b 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 @@ -3,9 +3,7 @@ from bitbots_tf_buffer import Buffer from geometry_msgs.msg import PointStamped, PoseStamped, PoseWithCovarianceStamped, Twist from nav_msgs.msg import Path -from rclpy.callback_groups import MutuallyExclusiveCallbackGroup from rclpy.duration import Duration -from rclpy.executors import MultiThreadedExecutor from std_msgs.msg import Bool, Empty from visualization_msgs.msg import MarkerArray @@ -31,36 +29,15 @@ def __init__(self) -> None: self.measurements = dict() # Subscriber - self.create_subscription( - PoseWithCovarianceStamped, - self.config.map.ball_update_topic, - self.planner.set_ball, - 5, - callback_group=MutuallyExclusiveCallbackGroup(), - ) - self.create_subscription( - sv3dm.RobotArray, - self.config.map.robot_update_topic, - self.planner.set_robots, - 5, - callback_group=MutuallyExclusiveCallbackGroup(), - ) - self.goal_sub = self.create_subscription( - PoseStamped, "goal_pose", self.planner.set_goal, 5, callback_group=MutuallyExclusiveCallbackGroup() - ) - self.create_subscription( - Empty, - "pathfinding/cancel", - lambda _: self.planner.cancel_goal(), - 5, - callback_group=MutuallyExclusiveCallbackGroup(), - ) + self.create_subscription(PoseWithCovarianceStamped, self.config.map.ball_update_topic, self.planner.set_ball, 5) + self.create_subscription(sv3dm.RobotArray, self.config.map.robot_update_topic, self.planner.set_robots, 5) + self.goal_sub = self.create_subscription(PoseStamped, "goal_pose", self.planner.set_goal, 5) + self.create_subscription(Empty, "pathfinding/cancel", lambda _: self.planner.cancel_goal(), 5) self.create_subscription( Bool, "ball_obstacle_active", lambda msg: self.planner.avoid_ball(msg.data), # type: ignore 5, - callback_group=MutuallyExclusiveCallbackGroup(), ) # Publisher @@ -70,12 +47,7 @@ def __init__(self) -> None: self.graph_pub = self.create_publisher(MarkerArray, "visibility_graph", 1) # Timer that updates the path and command velocity at a given rate - self.create_timer( - 1 / self.config.rate, - self.step, - clock=self.get_clock(), - callback_group=MutuallyExclusiveCallbackGroup(), - ) + self.create_timer(1 / self.config.rate, self.step, clock=self.get_clock()) def step(self) -> None: """ @@ -104,10 +76,7 @@ def main(args=None): rclpy.init(args=args) node = PathPlanning() - # choose number of threads by number of callback_groups + 1 for simulation time - ex = MultiThreadedExecutor(num_threads=8) - ex.add_node(node) - ex.spin() + rclpy.spin(node) node.destroy_node() rclpy.shutdown() From 7f88a8326c9c1d1712050d15996b201d4a46666b Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Tue, 4 Mar 2025 19:18:27 +0100 Subject: [PATCH 46/47] Always show markers --- bitbots_misc/bitbots_ipm/launch/ipm.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bitbots_misc/bitbots_ipm/launch/ipm.launch b/bitbots_misc/bitbots_ipm/launch/ipm.launch index 81b264066..efdf028c9 100644 --- a/bitbots_misc/bitbots_ipm/launch/ipm.launch +++ b/bitbots_misc/bitbots_ipm/launch/ipm.launch @@ -2,7 +2,7 @@ - + From 781c7259939bc552d75aff1d1af9066987b80d68 Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Tue, 4 Mar 2025 19:18:49 +0100 Subject: [PATCH 47/47] Adapt API --- .../bitbots_path_planning/bitbots_path_planning/planner.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) 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 53f4ff4e1..8e2d9388d 100644 --- a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/planner.py +++ b/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/planner.py @@ -125,7 +125,11 @@ def step(self) -> Path: start = (my_position.x, my_position.y) # Configure how obstacles are represented - config = ObstacleMapConfig(dilate=self.node.config.map.inflation.dilate, num_vertices=12) + config = ObstacleMapConfig( + robot_radius=self.node.config.map.inflation.robot_radius, + margin=self.node.config.map.inflation.obstacle_margin, + num_vertices=12, + ) # Add robots to obstacles obstacles = self.robots.copy() # Add ball to obstacles if active