Merge branch 'development' into test/easter_test

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,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,12 +333,15 @@ 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:
@@ -358,7 +360,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 +373,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,29 +382,21 @@ 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), z=0.0))
         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.
 
         Args:
             start (DSLNode): The start DSLNode.
             goal (DSLNode): The goal DSLNode.
-        
-        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()
 

guidance/d_star_lite/tests/test_d_star_lite.py

@@ -2,6 +2,7 @@
 import pytest
 from d_star_lite.d_star_lite import DStarLite
 from d_star_lite.d_star_lite_node import DSLNode
+from geometry_msgs.msg import Point
 import numpy as np
 
 class TestDStarLite(unittest.TestCase):
@@ -13,15 +14,14 @@ def setUp(self):
         self.DSLNode5 = DSLNode(-1, -1, -5.8)
 
         # Create example obstacle coordinates
-        self.ox = [1, 2, 3, 4, 5]
-        self.oy = [0, 0, 0, 0, 0]
+        self.obstacles = [Point(x=5.0, y=5.0, z=0.0)]
 
         # Create start and goal
-        self.start = DSLNode(0, 0)
-        self.goal = DSLNode(5, 5)
+        self.start = Point(x=0.0, y=0.0, z=0.0)
+        self.goal = Point(x=10.0, y=10.0, z=0.0)
 
         # Create example dsl object
-        self.dsl = DStarLite(self.ox, self.oy, self.start, self.goal)
+        self.dsl = DStarLite(self.obstacles, self.start, self.goal, min_dist_to_obstacle=3, origin=Point(x=0.0, y=0.0, z=0.0), height=30, width=30)
 
     def tearDown(self):
         pass
@@ -56,10 +56,10 @@ def test_distance_between_two_nodes_yields_correct_euclidean_distance(self):
     # Test the is_obstacle function
     def test_is_obstacle_is_true_when_node_is_obstacle(self):
 
-        self.assertEqual(self.dsl.is_obstacle(DSLNode(1, 0)), True)
+        self.assertEqual(self.dsl.is_obstacle(DSLNode(5, 5)), True)
 
     def test_is_obstacle_is_true_when_node_is_in_safe_distance_from_obstacle(self):
-        self.assertEqual(self.dsl.is_obstacle(DSLNode(2, 2)), True)
+        self.assertEqual(self.dsl.is_obstacle(DSLNode(4, 4)), True)
 
     def test_is_obstacle_is_false_when_node_is_not_obstacle(self):
         self.assertEqual(self.dsl.is_obstacle(DSLNode(10, 0)), False)
@@ -73,13 +73,13 @@ def test_movement_cost_when_moving_diagonally(self):
         self.assertEqual(self.dsl.movement_cost(DSLNode(10, 10), DSLNode(11, 11)), np.sqrt(2))
 
     def test_movement_cost_when_moving_to_obstacle(self):
-        self.assertEqual(self.dsl.movement_cost(DSLNode(1, 0), DSLNode(2, 0)), np.inf)
+        self.assertEqual(self.dsl.movement_cost(DSLNode(3, 3), DSLNode(4, 4)), np.inf)
 
     # Test the heuristic_distance function (distance to target node)
     def test_heuristic_distance(self):
-        self.assertEqual(self.dsl.heuristic_distance(DSLNode(0, 0)), np.sqrt(50))
-        self.assertEqual(self.dsl.heuristic_distance(DSLNode(5, 5)), 0.0)
-        self.assertEqual(self.dsl.heuristic_distance(DSLNode(10, 10)), np.sqrt(50))
+        self.assertEqual(self.dsl.heuristic_distance(DSLNode(0, 0)), 2*np.sqrt(50))
+        self.assertEqual(self.dsl.heuristic_distance(DSLNode(5, 5)), np.sqrt(50))
+        self.assertEqual(self.dsl.heuristic_distance(DSLNode(10, 10)), 0)
     # Test the get_direction function
     def test_get_direction_when_moving_in_positive_x_direction(self):
         self.assertEqual(DSLNode.get_direction(DSLNode(0, 0), DSLNode(1, 0)), (1, 0))

mission/blackbox/README

@@ -0,0 +1,25 @@
+## Balackbox
+
+Logs data of all the important topics such as:
+/asv/power_sense_module/current
+/asv/power_sense_module/voltage
+/asv/temperature/ESC1
+/asv/temperature/ESC2
+/asv/temperature/ESC3
+/asv/temperature/ESC4
+/asv/temperature/ambient1
+/asv/temperature/ambient2
+internal/status/bms0
+internal/status/bms1
+/thrust/thruster_forces
+/pwm
+
+
+## Service file bootup
+
+To start the blackbox loging automaticaly every time on bootup just run this command:
+```
+./vortex-asv/add_service_files_to_bootup_sequence.sh
+```
+
+Enjoy :)