Local path planner

guidance/d_star_lite/CMakeLists.txt

@@ -0,0 +1,35 @@
+cmake_minimum_required(VERSION 3.8)
+project(d_star_lite)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake_python REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(vortex_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+
+ament_python_install_package(${PROJECT_NAME})
+
+install(DIRECTORY
+  launch
+  DESTINATION share/${PROJECT_NAME}
+)
+
+install(PROGRAMS
+  d_star_lite/ros_d_star_lite_node.py
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  find_package(ament_cmake_pytest REQUIRED)
+  ament_add_pytest_test(python_tests tests)
+  set(ament_cmake_copyright_FOUND TRUE)
+  set(ament_cmake_cpplint_FOUND TRUE)
+endif()
+
+ament_package()

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_node.py

@@ -0,0 +1,81 @@
+import math
+
+class DSLNode:
+    """
+    Represents a DSLNode in the grid.
+
+    Attributes:
+        x (int): The x-coordinate of the DSLNode.
+        y (int): The y-coordinate of the DSLNode.
+        cost (float): The cost of moving to the DSLNode.
+
+    """
+    def __init__(self, x: int = 0, y: int = 0, cost: float = 0.0):
+        """
+        Initializes a new instance of the DSLNode class.
+
+        Args:
+            x (int): The x-coordinate of the DSLNode. Defaults to 0.
+            y (int): The y-coordinate of the DSLNode. Defaults to 0.
+            cost (float): The cost of moving to the DSLNode. Defaults to 0.0.
+        """
+        self.x = x
+        self.y = y
+        self.cost = cost
+
+    def __add__(self, other: 'DSLNode') -> 'DSLNode':
+        """
+        Adds two DSLNode objects together.
+
+        Args:
+            other (DSLNode): The DSLNode to add to the current DSLNode.
+
+        Returns:
+            DSLNode: A new DSLNode object with the combined x and y coordinates and costs.
+        """
+        return DSLNode(self.x + other.x, self.y + other.y, self.cost + other.cost)
+
+    def __eq__(self, other: 'DSLNode') -> bool:
+        """
+        Checks if two DSLNode objects have the same x and y coordinates.
+
+        Args:
+            other (DSLNode): The DSLNode to compare to the current DSLNode.
+
+        Returns:
+            bool: True if the nodes have the same x and y coordinates, False otherwise.
+        """
+        return self.x == other.x and self.y == other.y
+
+    @staticmethod
+    def distance_between_nodes(node1: 'DSLNode', node2: 'DSLNode') -> float:
+        """
+        Computes the Euclidean distance between two DSLNode objects.
+
+        Args:
+            node1 (DSLNode): The first DSLNode.
+            node2 (DSLNode): The second DSLNode.
+
+        Returns:
+            float: The Euclidean distance between the two nodes.
+        """
+        return math.sqrt((node1.x - node2.x)**2 + (node1.y - node2.y)**2)
+
+    @staticmethod
+    def get_direction(node1: 'DSLNode', node2: 'DSLNode') -> tuple:
+        """
+        Calculates the direction from node1 to node2.
+
+        Args:
+            node1 (DSLNode): The starting DSLNode.
+            node2 (DSLNode): The ending DSLNode.
+
+        Returns:
+            tuple: A tuple of two integers representing the direction from node1 to node2.
+        """
+        dx = node2.x - node1.x
+        dx = dx/abs(dx) if dx != 0 else 0
+        dy = node2.y - node1.y
+        dy = dy/abs(dy) if dy != 0 else 0
+        return dx, dy
+

guidance/d_star_lite/d_star_lite/ros_d_star_lite_node.py

@@ -0,0 +1,89 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+from d_star_lite.d_star_lite import DStarLite
+from vortex_msgs.srv import MissionParameters, Waypoint
+
+class DStarLiteNode(Node):
+    """
+    A ROS2 node implementing the D* Lite algorithm.
+
+    The node offers a mission planner service to calculate the optimal waypoints, which are then sent to the waypoint service.
+    """
+    def __init__(self):
+        """
+        Initialize the DStarLiteNode, creating necessary services and clients
+        for mission planning and waypoint submission.
+        """
+        super().__init__('d_star_lite_node')
+        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')
+
+
+    def d_star_lite_callback(self, request, response):
+        """
+        Callback for the mission planner service.
+
+        Args:
+            request: start and goal coordinates, the obstacle coordinates and the world boundaries
+            response: success flag
+
+        Returns:
+            The modified response object with success status
+        """
+        self.get_logger().info('D Star Lite Service has been called')
+        obstacles = request.obstacles
+        start = request.start
+        goal = request.goal
+        origin = request.origin
+        height = request.height
+        width = request.width
+
+        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()
+
+        # Send waypoints to waypoint service
+        self.send_waypoints_request()
+
+        response.success = True
+
+        return response
+
+    def send_waypoints_request(self):
+        """
+        Sends the computed waypoints to the waypoint service.
+        """
+        if not self.waypoint_client.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info('Waypoint service not available, waiting again...')
+            return None  # Return None to indicate the service is not available.
+        request = Waypoint.Request()
+        request.waypoint = self.waypoints
+        future = self.waypoint_client.call_async(request)
+        future.add_done_callback(self.waypoint_response_callback)  # Handle response
+
+    def waypoint_response_callback(self, future):
+        try:
+            response = future.result()
+            if response.success:
+                self.get_logger().info('Waypoints successfully submitted.')
+            else:
+                self.get_logger().error('Waypoint submission failed.')
+        except Exception as e:
+            self.get_logger().error('Service call failed %r' % (e,))
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = DStarLiteNode()
+    rclpy.spin(node)
+
+    # Cleanup and shutdown
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

guidance/d_star_lite/launch/d_star_lite.launch.py

@@ -0,0 +1,17 @@
+import os
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+
+def generate_launch_description():
+
+    d_star_lite_node = Node(
+        package='d_star_lite',
+        executable='d_star_lite_node.py',
+        name='d_star_lite_node',
+        output='screen'
+    )
+
+    return LaunchDescription([
+        d_star_lite_node
+    ])

guidance/d_star_lite/package.xml

@@ -0,0 +1,30 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>d_star_lite</name>
+  <version>0.0.0</version>
+  <description>d_star_lite algorithm for path planning</description>
+  <maintainer email="[email protected]">andeshog</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+  <test_depend>launch_testing_ament_cmake</test_depend>
+  <test_depend>launch_testing_ros</test_depend>
+
+  <depend>rclpy</depend>
+  <depend>std_msgs</depend>
+  <depend>vortex_msgs</depend>
+  <depend>geometry_msgs</depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

guidance/d_star_lite/tests/test_d_star_lite.py

@@ -0,0 +1,100 @@
+import unittest
+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):
+
+    def setUp(self):
+        # Create example DSLNodes
+        self.DSLNode1 = DSLNode(0, 0, 2.0)
+        self.DSLNode2 = DSLNode(1, 1, 3.4)
+        self.DSLNode5 = DSLNode(-1, -1, -5.8)
+
+        # Create example obstacle coordinates
+        self.obstacles = [Point(x=5.0, y=5.0, z=0.0)]
+
+        # Create start and goal
+        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.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
+
+    # Test node addition
+    def test_combine_DSLNodes_with_positive_values(self):
+        self.assertEqual(self.DSLNode1 + self.DSLNode2, DSLNode(1, 1, 5.4))
+
+    def test_combine_DSLNodes_with_negative_values(self):
+        self.assertEqual(self.DSLNode5 + self.DSLNode5, DSLNode(-2, -2, -11.6))
+
+    def test_combine_DSLNodes_with_positive_and_negative_values(self):
+        self.assertEqual(self.DSLNode1 + self.DSLNode5, DSLNode(-1, -1, -3.8))
+
+    # Test node comparison
+    def test_compare_coordinates_is_false_for_two_different_nodes(self):
+
+        self.assertEqual(self.DSLNode1 == self.DSLNode2, False)
+
+    def test_compare_coordinates_is_true_for_two_same_nodes(self):
+        self.assertEqual(self.DSLNode1 == self.DSLNode1, True)
+
+    # Test the distance function
+    def test_distance_between_two_nodes_yields_correct_euclidean_distance(self):
+
+        result = DSLNode.distance_between_nodes(self.DSLNode1, self.DSLNode2)
+        self.assertEqual(result, np.sqrt(2))
+
+        result = DSLNode.distance_between_nodes(self.DSLNode2, self.DSLNode5)
+        self.assertEqual(result, np.sqrt(8))
+
+    # Test the is_obstacle function
+    def test_is_obstacle_is_true_when_node_is_obstacle(self):
+
+        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(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)
+
+    # Test the movement_cost function
+    def test_movement_cost_when_moving_straight(self):
+
+        self.assertEqual(self.dsl.movement_cost(DSLNode(10, 0), DSLNode(11, 0)), 1.0)
+
+    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(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)), 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))
+
+    def test_get_direction_when_moving_positive_diagonal(self):
+        self.assertEqual(DSLNode.get_direction(DSLNode(0, 0), DSLNode(2, 2)), (1, 1))
+
+    def test_get_direction_when_moving_in_negative_x_direction(self):
+        self.assertEqual(DSLNode.get_direction(DSLNode(0, 0), DSLNode(-1, 0)), (-1, 0))
+
+    def test_get_direction_when_moving_in_negative_y_direction(self):
+        self.assertEqual(DSLNode.get_direction(DSLNode(0, 0), DSLNode(0, -1)), (0, -1))
+
+    def test_get_direction_when_moving_in_negative_diagonal(self):
+        self.assertEqual(DSLNode.get_direction(DSLNode(0, 0), DSLNode(-2, -2)), (-1, -1))
+
+if __name__ == '__main__':
+    unittest.main()