Local path planner

guidance/d_star_lite/d_star_lite/README.md → guidance/d_star_lite/README.md

@@ -9,13 +9,13 @@ And when using this map when running the D* lite algorithm with safe distance 4.
 
 A D* lite object consists of these parameters:
 
-'''
-dsl_object = DStarLite(obstacle_x, obstacle_y, start, goa, dist_to_obstacle, origin, height, width)
-'''
+```
+dsl_object = DStarLite(obstacle_x, obstacle_y, start, goal, safe_dist_to_obstacle, origin, height, width)
+```
 
-where 'obstacle_x' and 'obstacle_y' are lists containg the x and y coordinates of the obstacles. 'start' and 'goal' are the start and goal node for the selected mission. 'origin', 'height' and 'widht' are parameters to create the world boundaries and are used to compute 'x_min', 'x_max', 'y_min' and 'y_max'. See the figures below for visual representation.
+where `obstacle_x` and `obstacle_y` are lists containg the x and y coordinates of the obstacles. `start` and `goal` are the start and goal node for the selected mission. `origin`, `height` and `width` are parameters to create the world boundaries and are used to compute `x_min`, `x_max`, `y_min` and `y_max`. See the figures below for visual representation.
 
-![World Grid](https://drive.google.com/uc?export=download&id=1aoxujTgjgO8oaP2H6xIE0JHGb6VVNcRs)
+![World Grid](https://drive.google.com/uc?export=download&id=1RYXcRTjFWMFRhYBMRx5ILmheNvEKahrY)
 
 ![Obstacle](https://drive.google.com/uc?export=download&id=1M43ohD3wpKwmgkjJ44Ut1uOT3f5MlSQI)
 

guidance/d_star_lite/d_star_lite/d_star_lite.py

@@ -1,7 +1,7 @@
 import numpy as np
 import math
-import matplotlib.pyplot as plt
 from d_star_lite.d_star_lite_node import DSLNode
+from geometry_msgs.msg import Point
 
 # Link to the original code:
 # https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning/DStarLite/d_star_lite.py
@@ -18,7 +18,7 @@ class DStarLite:
 
     Methods:
     -------------------------------------------------------------------------------------------
-        __init__(ox: list, oy: list, dist_to_obstacle: float = 4.5): Initializes a new instance of the DStarLite class.
+        __init__(ox: list, oy: list, min_dist_to_obstacle: float = 4.5): Initializes a new instance of the DStarLite class.
 
         create_grid(val: float) -> np.ndarray: Creates a grid initialized with a specific value.
 
@@ -51,48 +51,45 @@ class DStarLite:
         dsl_main(start: DSLNode, goal: DSLNode) -> tuple[bool, list[int], list[int]]: Main function to run the D* Lite algorithm.
     """
 
-    motions = [ # Represents the possible motions in the grid and corresponding costs
+    possible_motions = [ # Represents the possible motions in the grid and corresponding costs
         DSLNode(1, 0, 1), DSLNode(0, 1, 1), DSLNode(-1, 0, 1), DSLNode(0, -1, 1), 
         DSLNode(1, 1, math.sqrt(2)), DSLNode(1, -1, math.sqrt(2)),
         DSLNode(-1, 1, math.sqrt(2)), DSLNode(-1, -1, math.sqrt(2))
     ]
 
-    def __init__(self, ox: list, oy: list, start: DSLNode, goal: DSLNode, dist_to_obstacle: float = 4.5, origin: tuple = (0,0), height: int = 25, width: int = 25):
+    def __init__(self, obstacles: list[Point], start: Point, goal: Point, min_dist_to_obstacle: float = 4.5, origin: Point = Point(x=0.0,y=0.0), height: int = 25, width: int = 25):
         """
         Initializes a new instance of the DStarLite class.
 
         Args:
-            ox (list): The x-coordinates of the obstacles.
-            oy (list): The y-coordinates of the obstacles.
+            obstacles (list): A list of Point objects representing the obstacles.
             start (DSLNode): The start node.
             goal (DSLNode): The goal node.
-            dist_to_obstacle (float): The minimum distance a DSLNode must be from any obstacle to be considered valid. Defaults to 4.5.
+            min_dist_to_obstacle (float): The minimum distance a DSLNode must be from any obstacle to be considered valid. Defaults to 4.5.
             origin (tuple): The origin of the grid. Defaults to (0, 0).
             height (int): The height of the grid. Defaults to 25.
             width (int): The width of the grid. Defaults to 25.
         """
-        if len(ox) != len(oy):
-            raise ValueError("The number of x and y coordinates must be equal.")
-
-        if len(ox) == 0 and len(oy) == 0: # If no obstacles are provided
+        if len(obstacles) == 0: # If no obstacles are provided
             self.obstacles = []
+
         else:
-            self.obstacles = [DSLNode(x, y) for x, y in zip(ox, oy)] # The obstacles as nodes
+            self.obstacles = [DSLNode(int(point.x), int(point.y)) for point in obstacles] # The obstacles as nodes
             self.obstacles_xy = np.array( # The obstacles as xy coordinates
                 [[obstacle.x, obstacle.y] for obstacle in self.obstacles]
             )
 
-        self.start = start # The start DSLNode
-        self.goal = goal # The goal DSLNode
-        self.origin = origin # The origin of the world grid
-        self.height = height # The height of the world grid
-        self.width = width # The width of the world grid
+        self.start = DSLNode(int(start.x), int(start.y)) # The start DSLNode
+        self.goal = DSLNode(int(goal.x), int(goal.y)) # The goal DSLNode
+        self.world_grid_origin = (int(origin.x), int(origin.y)) # The origin of the world grid
+        self.world_grid_height = height/2 # The height of the world grid
+        self.world_grid_width = width/2 # The width of the world grid
 
         # Compute the min and max values for the world boundaries
-        self.x_min = int(origin[0]-self.width)
-        self.y_min = int(origin[1]-self.height)
-        self.x_max = int(origin[0]+self.width)
-        self.y_max = int(origin[1]+self.height)
+        self.x_min = int(origin.x-self.world_grid_width)
+        self.y_min = int(origin.y-self.world_grid_height)
+        self.x_max = int(origin.x+self.world_grid_width)
+        self.y_max = int(origin.y+self.world_grid_height)
 
         self.priority_queue = [] # Priority queue
         self.key_min = 0.0 # The minimum key in priority queue
@@ -101,7 +98,7 @@ def __init__(self, ox: list, oy: list, start: DSLNode, goal: DSLNode, dist_to_ob
         self.g = self.create_grid(float("inf")) # The g values
         self.initialized = False # Whether the grid has been initialized
         self.waypoints = [] # The waypoints
-        self.dist_to_obstacle = dist_to_obstacle # The minimum distance a DSLNode must be from any obstacle to be considered valid
+        self.min_dist_to_obstacle = min_dist_to_obstacle # The minimum distance a DSLNode must be from any obstacle to be considered valid
 
     def create_grid(self, val: float) -> np.ndarray:
         """
@@ -113,7 +110,7 @@ def create_grid(self, val: float) -> np.ndarray:
         Returns:
             np.ndarray: A 2D numpy array representing the initialized grid.
         """
-        return np.full((self.x_max, self.y_max), val)
+        return np.full((self.x_max - self.x_min, self.y_max - self.y_min), val)
 
     def is_obstacle(self, dslnode: DSLNode) -> bool:
         """
@@ -136,7 +133,7 @@ def is_obstacle(self, dslnode: DSLNode) -> bool:
         distances = np.sqrt(np.sum((self.obstacles_xy - node_xy) ** 2, axis=1))
 
         # Check if any distance is less than the minimum distance (default: 4.5)
-        return np.any(distances < self.dist_to_obstacle)
+        return np.any(distances < self.min_dist_to_obstacle)
 
     def movement_cost(self, node1: DSLNode, node2: DSLNode) -> float:
         """
@@ -153,7 +150,7 @@ def movement_cost(self, node1: DSLNode, node2: DSLNode) -> float:
             return math.inf
 
         movement_vector = DSLNode(node1.x - node2.x, node1.y - node2.y)
-        for motion in self.motions:
+        for motion in self.possible_motions:
             if motion == movement_vector:
                 return motion.cost
         return math.inf
@@ -198,7 +195,7 @@ def is_valid(self, DSLNode: DSLNode) -> bool:
         Returns:
             bool: True if the DSLNode is within the grid boundaries, False otherwise.
         """
-        return 0 <= DSLNode.x < self.x_max and 0 <= DSLNode.y < self.y_max
+        return self.x_min <= DSLNode.x < self.x_max and self.y_min <= DSLNode.y < self.y_max
 
     def pred(self, u: DSLNode) -> list[DSLNode]:
         """
@@ -210,7 +207,7 @@ def pred(self, u: DSLNode) -> list[DSLNode]:
         Returns:
             list: A list of predecessors of the DSLNode 'u'.
         """
-        return [u + motion for motion in self.motions if self.is_valid(u + motion)]
+        return [u + motion for motion in self.possible_motions if self.is_valid(u + motion)]
 
     def initialize(self):
         """
@@ -319,8 +316,10 @@ def compute_shortest_path(self):
             # Double-check conditions to potentially exit the loop
             has_elements = len(self.priority_queue) > 0
             start_key = self.calculate_key(self.start)
+
             # Determine if the start node's key is outdated or not, affecting loop continuation
             start_key_not_updated = has_elements and self.compare_keys(self.priority_queue[0][1], start_key)
+
             # Check if the rhs value and g value for the start node are equal, indicating optimality reached for the start node
             rhs_not_equal_to_g = self.rhs[self.start.x][self.start.y] != self.g[self.start.x][self.start.y]
 
@@ -334,16 +333,20 @@ def compute_shortest_path(self):
             # If the current node's old key is outdated, reinsert it with the updated key
             if self.compare_keys(k_old, self.calculate_key(u)):
                 self.priority_queue.append((u, self.calculate_key(u)))
+
             # If the g value is greater than the rhs value, update it to achieve consistency
             elif self.g[u.x][u.y] > self.rhs[u.x][u.y]:
                 self.g[u.x][u.y] = self.rhs[u.x][u.y]
+
                 # Update all predecessors of the current node as their cost might have changed
                 for s in self.pred(u):
                     self.update_vertex(s)
+
             # If no optimal path is known (g value is infinity), set the current node's g value to
             # infinity and update its predecessors
             else:
                 self.g[u.x][u.y] = float('inf')
+
                 # Update the current node and its predecessors
                 for s in self.pred(u) + [u]:
                     self.update_vertex(s)
@@ -358,7 +361,7 @@ def compute_current_path(self) -> list[DSLNode]:
         path = list()
         current_point = DSLNode(self.start.x, self.start.y)
         last_point = None
-
+        
         while not current_point == self.goal:
             if last_point is not None:
                 self.detect_and_update_waypoints(last_point, current_point)
@@ -371,7 +374,7 @@ def compute_current_path(self) -> list[DSLNode]:
         return path
 
 
-    def get_WP(self) -> list[list[int]]:
+    def get_WP(self) -> list[Point]:
         """
         Retrieves the waypoints and adjusts their coordinates to the original coordinate system.
 
@@ -380,11 +383,11 @@ def get_WP(self) -> list[list[int]]:
         """
         WP_list = []
         for wp in self.waypoints:
-            WP_list.append([wp.x, wp.y])
+            WP_list.append(Point(x=float(wp.x), y=float(wp.y)))
         return WP_list
 
 
-    def dsl_main(self):# -> tuple[bool, list[int], list[int]]:
+    def dsl_main(self) -> None:
         """
         Main function to run the D* Lite algorithm.
 
@@ -395,14 +398,9 @@ def dsl_main(self):# -> tuple[bool, list[int], list[int]]:
         Returns:
             tuple: A tuple containing a boolean indicating if the path was found, and the x and y coordinates of the path.  
         """ 
-        pathx = []
-        pathy = []
         self.initialize()
         self.compute_shortest_path()
-        path = self.compute_current_path()
-        # pathx.append(self.start.x + self.x_min)
-        # pathy.append(self.start.y + self.y_min)
-        pathx = [node.x for node in path]
-        pathy = [node.y for node in path]
+        self.compute_current_path()
         print("Path found")
+
 

guidance/d_star_lite/d_star_lite/ros_d_star_lite_node.py

@@ -1,13 +1,9 @@
 #!/usr/bin/env python3
 
-import sys
 import rclpy
 from rclpy.node import Node
-import numpy as np
 from d_star_lite.d_star_lite import DStarLite
-from d_star_lite.d_star_lite_node import DSLNode
 from vortex_msgs.srv import MissionParameters, Waypoint
-from geometry_msgs.msg import Point
 
 class DStarLiteNode(Node):
     """
@@ -24,7 +20,6 @@ def __init__(self):
         self.obstacle_srv = self.create_service(MissionParameters, 'mission_parameters', self.d_star_lite_callback)
         self.waypoint_client = self.create_client(Waypoint, 'waypoint')
         self.get_logger().info('D Star Lite Node has been started')
-        self.waypoint_list = []
 
 
     def d_star_lite_callback(self, request, response):
@@ -40,23 +35,18 @@ def d_star_lite_callback(self, request, response):
         """
         self.get_logger().info('D Star Lite Service has been called')
         obstacles = request.obstacles
-        obstacles_x = [obs.x for obs in obstacles]
-        obstacles_y = [obs.y for obs in obstacles]
         start = request.start
         goal = request.goal
         origin = request.origin
         height = request.height
         width = request.width
-        start_node = DSLNode(int(start.x), int(start.y))
-        goal_node = DSLNode(int(goal.x), int(goal.y))
-        origin = (origin.x, origin.y)
 
-        dsl = DStarLite(obstacles_x, obstacles_y, start_node, goal_node, origin=origin, height=height, width=width)
+        dsl = DStarLite(obstacles, start, goal, origin=origin, height=height, width=width)
         dsl.dsl_main() # Run the main function to generate path
 
-        waypoints_list = np.array(dsl.get_WP()).tolist()
-        # Convert to Point2D[] for Waypoint service
-        self.waypoints = [Point(x=float(wp[0]), y=float(wp[1])) for wp in waypoints_list]
+        # Get waypoints
+        self.waypoints = dsl.get_WP()
+
         # Send waypoints to waypoint service
         self.send_waypoints_request()
 

mission/mission_planner/mission_planner/mission_planner.py

@@ -1,6 +1,5 @@
 #!/usr/bin/env python3
 
-import sys
 import rclpy
 from rclpy.node import Node
 from vortex_msgs.srv import MissionParameters
@@ -52,10 +51,10 @@ def main(args=None):
     rclpy.init(args=args)
     mission_planner_client = MissionPlannerClient()
     # Test data
-    obstacles = []
-    start = Point(x=1.0, y=1.0)
+    obstacles = [Point(x=5.0, y=5.0)]
+    start = Point(x=0.0, y=0.0)
     goal = Point(x=10.0, y=10.0)
-    mission_planner_client.send_request(obstacles, start, goal, Point(x=0.0, y=0.0), 15, 15)
+    mission_planner_client.send_request(obstacles, start, goal, Point(x=0.0, y=0.0), 30, 30)
 
     while rclpy.ok():
         rclpy.spin_once(mission_planner_client)