fixed merge conflict

acoustics/acoustics_data_record/acoustics_data/.gitignore

@@ -0,0 +1 @@
+*

guidance/d_star_lite/README.md

@@ -0,0 +1,24 @@
+# D* lite
+Given a map of obstacles and a minimum safe distance to the obstacles, this algorithm will compute the shortest path from the start point to the end point. This is an example of an obstacle map:
+
+![Obstacle Map](https://drive.google.com/uc?export=download&id=1MohnPBQMoQHHbLaDkbUe43MjBPp5sTiF)
+
+And when using this map when running the D* lite algorithm with safe distance 4.5 we get this path:
+
+![Path](https://drive.google.com/uc?export=download&id=1il-i2aJM3pkQacTO8Jg77_2zDVcZF1ty)
+
+A D* lite object consists of these parameters:
+
+```
+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 `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=1RYXcRTjFWMFRhYBMRx5ILmheNvEKahrY)
+
+![Obstacle](https://drive.google.com/uc?export=download&id=1M43ohD3wpKwmgkjJ44Ut1uOT3f5MlSQI)
+
+# D* lite ROS2 node
+The node is responsible for gathering the waypoints found from the algorithm and send them to the waypoint manager. The node receives the mission parameters from the mission planner. It is both a client and a server.
+

guidance/d_star_lite/d_star_lite/d_star_lite.py

@@ -1,7 +1,12 @@
 import numpy as np
 import math
+<<<<<<< HEAD
 import matplotlib.pyplot as plt
 from d_star_lite.d_star_lite_node import DSLNode
+=======
+from d_star_lite.d_star_lite_node import DSLNode
+from geometry_msgs.msg import Point
+>>>>>>> development
 
 # Link to the original code:
 # https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning/DStarLite/d_star_lite.py
@@ -18,7 +23,11 @@ class DStarLite:
     
     Methods:
     -------------------------------------------------------------------------------------------
+<<<<<<< HEAD
         __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.
+>>>>>>> development
         
         create_grid(val: float) -> np.ndarray: Creates a grid initialized with a specific value.
         
@@ -51,37 +60,61 @@ class DStarLite:
         dsl_main(start: DSLNode, goal: DSLNode) -> tuple[bool, list[int], list[int]]: Main function to run the D* Lite algorithm.
     """
 
+<<<<<<< HEAD
     motions = [ # Represents the possible motions in the grid and corresponding costs
+=======
+    possible_motions = [ # Represents the possible motions in the grid and corresponding costs
+>>>>>>> development
         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))
     ]
 
+<<<<<<< HEAD
     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):
+>>>>>>> development
         """
         Initializes a new instance of the DStarLite class.
 
         Args:
+<<<<<<< HEAD
             ox (list): The x-coordinates of the obstacles.
             oy (list): The y-coordinates of 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.
+=======
+            obstacles (list): A list of Point objects representing the obstacles.
+            start (DSLNode): The start node.
+            goal (DSLNode): The goal node.
+            min_dist_to_obstacle (float): The minimum distance a DSLNode must be from any obstacle to be considered valid. Defaults to 4.5.
+>>>>>>> development
             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.
         """
+<<<<<<< HEAD
         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
             self.obstacles = []
         else:
             self.obstacles = [DSLNode(x, y) for x, y in zip(ox, oy)] # The obstacles as nodes
+=======
+        if len(obstacles) == 0: # If no obstacles are provided
+            self.obstacles = []
+
+        else:
+            self.obstacles = [DSLNode(int(point.x), int(point.y)) for point in obstacles] # The obstacles as nodes
+>>>>>>> development
             self.obstacles_xy = np.array( # The obstacles as xy coordinates
                 [[obstacle.x, obstacle.y] for obstacle in self.obstacles]
             )
 
+<<<<<<< HEAD
         self.start = start # The start DSLNode
         self.goal = goal # The goal DSLNode
         self.origin = origin # The origin of the world grid
@@ -93,6 +126,19 @@ def __init__(self, ox: list, oy: list, start: DSLNode, goal: DSLNode, dist_to_ob
         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.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.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)
+>>>>>>> development
 
         self.priority_queue = [] # Priority queue
         self.key_min = 0.0 # The minimum key in priority queue
@@ -101,7 +147,11 @@ 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
+<<<<<<< HEAD
         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
+>>>>>>> development
 
     def create_grid(self, val: float) -> np.ndarray:
         """
@@ -113,7 +163,11 @@ def create_grid(self, val: float) -> np.ndarray:
         Returns:
             np.ndarray: A 2D numpy array representing the initialized grid.
         """
+<<<<<<< HEAD
         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)
+>>>>>>> development
 
     def is_obstacle(self, dslnode: DSLNode) -> bool:
         """
@@ -136,7 +190,11 @@ 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)
+<<<<<<< HEAD
         return np.any(distances < self.dist_to_obstacle)
+=======
+        return np.any(distances < self.min_dist_to_obstacle)
+>>>>>>> development
 
     def movement_cost(self, node1: DSLNode, node2: DSLNode) -> float:
         """
@@ -153,7 +211,11 @@ def movement_cost(self, node1: DSLNode, node2: DSLNode) -> float:
             return math.inf
 
         movement_vector = DSLNode(node1.x - node2.x, node1.y - node2.y)
+<<<<<<< HEAD
         for motion in self.motions:
+=======
+        for motion in self.possible_motions:
+>>>>>>> development
             if motion == movement_vector:
                 return motion.cost
         return math.inf
@@ -198,7 +260,11 @@ def is_valid(self, DSLNode: DSLNode) -> bool:
         Returns:
             bool: True if the DSLNode is within the grid boundaries, False otherwise.
         """
+<<<<<<< HEAD
         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
+>>>>>>> development
 
     def pred(self, u: DSLNode) -> list[DSLNode]:
         """
@@ -210,7 +276,11 @@ def pred(self, u: DSLNode) -> list[DSLNode]:
         Returns:
             list: A list of predecessors of the DSLNode 'u'.
         """
+<<<<<<< HEAD
         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)]
+>>>>>>> development
 
     def initialize(self):
         """
@@ -319,8 +389,15 @@ 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)
+<<<<<<< HEAD
             # 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)
+=======
+
+            # 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)
+
+>>>>>>> development
             # 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 +411,32 @@ 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)))
+<<<<<<< HEAD
             # 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 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)
+
+>>>>>>> development
             # 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')
+<<<<<<< HEAD
+=======
+
+>>>>>>> development
                 # Update the current node and its predecessors
                 for s in self.pred(u) + [u]:
                     self.update_vertex(s)
@@ -358,7 +451,11 @@ def compute_current_path(self) -> list[DSLNode]:
         path = list()
         current_point = DSLNode(self.start.x, self.start.y)
         last_point = None
+<<<<<<< HEAD
 
+=======
+
+>>>>>>> development
         while not current_point == self.goal:
             if last_point is not None:
                 self.detect_and_update_waypoints(last_point, current_point)
@@ -371,7 +468,11 @@ def compute_current_path(self) -> list[DSLNode]:
         return path
 
 
+<<<<<<< HEAD
     def get_WP(self) -> list[list[int]]:
+=======
+    def get_WP(self) -> list[Point]:
+>>>>>>> development
         """
         Retrieves the waypoints and adjusts their coordinates to the original coordinate system.
 
@@ -380,17 +481,26 @@ def get_WP(self) -> list[list[int]]:
         """
         WP_list = []
         for wp in self.waypoints:
+<<<<<<< HEAD
             WP_list.append([wp.x, wp.y])
         return WP_list
 
 
     def dsl_main(self):# -> tuple[bool, list[int], list[int]]:
+=======
+            WP_list.append(Point(x=float(wp.x), y=float(wp.y), z=0.0))
+        return WP_list
+
+
+    def dsl_main(self) -> None:
+>>>>>>> development
         """
         Main function to run the D* Lite algorithm.
 
         Args:
             start (DSLNode): The start DSLNode.
             goal (DSLNode): The goal DSLNode.
+<<<<<<< HEAD
         
         Returns:
             tuple: A tuple containing a boolean indicating if the path was found, and the x and y coordinates of the path.  
@@ -405,4 +515,12 @@ def dsl_main(self):# -> tuple[bool, list[int], list[int]]:
         pathx = [node.x for node in path]
         pathy = [node.y for node in path]
         print("Path found")
+=======
+        """
+        self.initialize()
+        self.compute_shortest_path()
+        self.compute_current_path()
+        print("Path found")
+
+>>>>>>> development
 

guidance/d_star_lite/d_star_lite/ros_d_star_lite_node.py

@@ -1,5 +1,6 @@
 #!/usr/bin/env python3
 
+<<<<<<< HEAD
 import sys
 import rclpy
 from rclpy.node import Node
@@ -8,6 +9,12 @@
 from d_star_lite.d_star_lite_node import DSLNode
 from vortex_msgs.srv import MissionParameters, Waypoint
 from geometry_msgs.msg import Point
+=======
+import rclpy
+from rclpy.node import Node
+from d_star_lite.d_star_lite import DStarLite
+from vortex_msgs.srv import MissionParameters, Waypoint
+>>>>>>> development
 
 class DStarLiteNode(Node):
     """
@@ -24,7 +31,10 @@ 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')
+<<<<<<< HEAD
         self.waypoint_list = []
+=======
+>>>>>>> development
 
 
     def d_star_lite_callback(self, request, response):
@@ -40,13 +50,17 @@ def d_star_lite_callback(self, request, response):
         """
         self.get_logger().info('D Star Lite Service has been called')
         obstacles = request.obstacles
+<<<<<<< HEAD
         obstacles_x = [obs.x for obs in obstacles]
         obstacles_y = [obs.y for obs in obstacles]
+=======
+>>>>>>> development
         start = request.start
         goal = request.goal
         origin = request.origin
         height = request.height
         width = request.width
+<<<<<<< HEAD
         start_node = DSLNode(int(start.x), int(start.y))
         goal_node = DSLNode(int(goal.x), int(goal.y))
         origin = (origin.x, origin.y)
@@ -57,6 +71,15 @@ def d_star_lite_callback(self, request, response):
         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]
+=======
+
+        dsl = DStarLite(obstacles, start, goal, origin=origin, height=height, width=width)
+        dsl.dsl_main() # Run the main function to generate path
+
+        # Get waypoints
+        self.waypoints = dsl.get_WP()
+
+>>>>>>> development
         # Send waypoints to waypoint service
         self.send_waypoints_request()