diff --git a/acoustics/acoustics_data_record/acoustics_data/.gitignore b/acoustics/acoustics_data_record/acoustics_data/.gitignore new file mode 100644 index 00000000..f59ec20a --- /dev/null +++ b/acoustics/acoustics_data_record/acoustics_data/.gitignore @@ -0,0 +1 @@ +* \ No newline at end of file diff --git a/guidance/d_star_lite/CMakeLists.txt b/guidance/d_star_lite/CMakeLists.txt new file mode 100644 index 00000000..7badbfb7 --- /dev/null +++ b/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() \ No newline at end of file diff --git a/guidance/d_star_lite/README.md b/guidance/d_star_lite/README.md new file mode 100644 index 00000000..cf7a35f6 --- /dev/null +++ b/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. + diff --git a/guidance/d_star_lite/d_star_lite/__init__.py b/guidance/d_star_lite/d_star_lite/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/guidance/d_star_lite/d_star_lite/d_star_lite.py b/guidance/d_star_lite/d_star_lite/d_star_lite.py new file mode 100755 index 00000000..d1d918b1 --- /dev/null +++ b/guidance/d_star_lite/d_star_lite/d_star_lite.py @@ -0,0 +1,403 @@ +import numpy as np +import math +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 + +# Link to theory: +# http://idm-lab.org/bib/abstracts/papers/aaai02b.pdf + + +class DStarLite: + """ + Implements the D* Lite algorithm for path planning in a grid. + + This class manages the pathfinding grid, obstacles and calculates the shortest path from a start DSLNode to a goal DSLNode. + + Methods: + ------------------------------------------------------------------------------------------- + __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. + + is_obstacle(DSLNode: DSLNode) -> bool: Check if the DSLNode is considered an obstacle or is too close to an obstacle. + + movement_cost(node1: DSLNode, node2: DSLNode) -> float: Calculates the cost of moving from node1 to node2. + + heuristic_distance(s: DSLNode) -> float: Calculates the heuristic distance from DSLNode s to the goal using the Euclidean distance. + + calculate_key(s: DSLNode) -> tuple: Calculates the priority key for a DSLNode 's' based on the D* Lite algorithm. + + is_valid(DSLNode: DSLNode) -> bool: Determines if a DSLNode is within the grid boundaries. + + pred(u: DSLNode) -> list[DSLNode]: Retrieves the predecessors of a DSLNode 'u'. + + initialize(start: DSLNode, goal: DSLNode): Initializes the grid and the D* Lite algorithm. + + update_vertex(u: DSLNode): Updates the vertex in the priority queue and the rhs value of the DSLNode 'u'. + + detect_and_update_waypoints(current_point: DSLNode, next_point: DSLNode): Updates the waypoints based on the current and next points. + + compare_keys(key_pair1: tuple[float, float], key_pair2: tuple[float, float]) -> bool: Compares the priority keys of two nodes. + + compute_shortest_path(): Computes or recomputes the shortest path from the start to the goal using the D* Lite algorithm. + + compute_current_path() -> list[DSLNode]: Computes the current path from the start to the goal. + + get_WP() -> list[list[int]]: Retrieves the waypoints and adjusts their coordinates to the original coordinate system. + + dsl_main(start: DSLNode, goal: DSLNode) -> tuple[bool, list[int], list[int]]: Main function to run the D* Lite algorithm. + """ + + 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, 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: + 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. + 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(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 + self.obstacles_xy = np.array( # The obstacles as xy coordinates + [[obstacle.x, obstacle.y] for obstacle in self.obstacles] + ) + + 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) + + self.priority_queue = [] # Priority queue + self.key_min = 0.0 # The minimum key in priority queue + self.key_old = 0.0 # The old minimum key in priority queue + self.rhs = self.create_grid(float("inf")) # The right hand side values + self.g = self.create_grid(float("inf")) # The g values + self.initialized = False # Whether the grid has been initialized + self.waypoints = [] # The waypoints + 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: + """ + Creates a grid initialized with a specific value. + + Args: + val (float): The value to initialize the grid with. + + Returns: + np.ndarray: A 2D numpy array representing the initialized grid. + """ + return np.full((self.x_max - self.x_min, self.y_max - self.y_min), val) + + def is_obstacle(self, dslnode: DSLNode) -> bool: + """ + Check if the DSLNode is considered an obstacle or is too close to an obstacle. + + Args: + DSLNode (DSLNode): The DSLNode to check. + + Returns: + bool: True if the DSLNode is too close to an obstacle or is an obstacle, False otherwise. + """ + # If there are no obstacles, return False + if len(self.obstacles) == 0: + return False + + # Convert the DSLNode's coordinates to a numpy array for efficient distance computation + node_xy = np.array([dslnode.x, dslnode.y]) + + # Compute the euclidean distances from the DSLNode to all obstacles + 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.min_dist_to_obstacle) + + def movement_cost(self, node1: DSLNode, node2: DSLNode) -> float: + """ + Calculates the cost of moving from node1 to node2. Returns infinity if node2 is an obstacle or if no valid motion is found. + + Args: + node1 (DSLNode): The starting DSLNode. + node2 (DSLNode): The ending DSLNode. + + Returns: + float: The cost of moving from node1 to node2. + """ + if self.is_obstacle(node2): + return math.inf + + movement_vector = DSLNode(node1.x - node2.x, node1.y - node2.y) + for motion in self.possible_motions: + if motion == movement_vector: + return motion.cost + return math.inf + + def heuristic_distance(self, s: DSLNode) -> float: + """ + Calculates the heuristic distance from DSLNode s to the goal using the Euclidean distance. + + Args: + s (DSLNode): The DSLNode to calculate the heuristic distance from. + + Returns: + float: The heuristic distance from DSLNode s to the goal. + """ + return DSLNode.distance_between_nodes(s, self.goal) # Euclidean distance + + def calculate_key(self, s: DSLNode) -> tuple: + """ + Calculates the priority key for a DSLNode 's' based on the D* Lite algorithm. + + The key is a tuple consisting of two parts: + 1. The estimated total cost from the start to the goal through 's', combining the minimum of g(s) and rhs(s), + the heuristic distance to the goal and a constant key_min. + 2. The minimum of g(s) and rhs(s) representing the best known cost to reach 's'. + + Args: + s (DSLNode): The DSLNode to calculate the key for. + + Returns: + tuple: A tuple of two floats representing the priority key for the DSLNode. + """ + return (min(self.g[s.x][s.y], self.rhs[s.x][s.y]) + self.heuristic_distance(s) + self.key_min, + min(self.g[s.x][s.y], self.rhs[s.x][s.y])) + + def is_valid(self, DSLNode: DSLNode) -> bool: + """ + Determines if a DSLNode is within the grid boundaries. + + Args: + DSLNode (DSLNode): The DSLNode to check. + + Returns: + bool: True if the DSLNode is within the grid boundaries, False otherwise. + """ + 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]: + """ + Retrieves the predecessors of a DSLNode 'u'. In this case, the predecessors are the valid neighbours of the DSLNode. + + Args: + u (DSLNode): The DSLNode to retrieve predecessors for. + + Returns: + list: A list of predecessors of the DSLNode 'u'. + """ + return [u + motion for motion in self.possible_motions if self.is_valid(u + motion)] + + def initialize(self): + """ + Initializes the grid and the D* Lite algorithm. + This function adjusts the coordinates of the start and goal nodes based on the grid's minimum world coordinates, + sets up the cost and right-hand side (rhs) grids, and initializes the priority queue. This setup is required + before the algorithm begins pathfinding. The initialization will only occur once; subsequent calls to this + function will have no effect. + + Args: + start (DSLNode): The start DSLNode. + goal (DSLNode): The goal DSLNode. + """ + + if not self.initialized: + self.initialized = True + print("Initializing") + self.priority_queue = [] + self.key_min = 0.0 + self.rhs = self.create_grid(math.inf) + self.g = self.create_grid(math.inf) + self.rhs[self.goal.x][self.goal.y] = 0 + self.priority_queue.append((self.goal, self.calculate_key(self.goal))) + + def update_vertex(self, u: DSLNode): + """ + Updates the vertex in the priority queue and the rhs value of the DSLNode 'u'. + + This method adjusts the right-hand side (rhs) value for a DSLNode unless it's the goal. It also ensures that the + DSLNode's priority in the queue reflects its current g and rhs values, reordering the queue as necessary. + + Args: + u (DSLNode): The DSLNode to update. + + """ + if not u == self.goal: + self.rhs[u.x][u.y] = min([self.movement_cost(u, sprime) + self.g[sprime.x][sprime.y] for sprime in self.pred(u)]) + + # Update the priority queue + if any([u == dslnode for dslnode, key in self.priority_queue]): + self.priority_queue = [(dslnode, key) for dslnode, key in self.priority_queue if not u == dslnode] + self.priority_queue.sort(key=lambda x: x[1]) + if self.g[u.x][u.y] != self.rhs[u.x][u.y]: + self.priority_queue.append((u, self.calculate_key(u))) + + # Resort the priority queue + self.priority_queue.sort(key=lambda x: x[1]) + + def detect_and_update_waypoints(self, current_point: DSLNode, next_point: DSLNode): + """ + Updates the waypoints based on the current and next points. + + This function checks the direction between the last waypoint and the current point, and the direction + between the current point and the next point. If there is a change in direction, indicating a turn or + deviation in the path, the current point is added to the list of waypoints. + + Args: + current_point (DSLNode): The current point. + next_point (DSLNode): The next point. + """ + if not self.waypoints: # If the waypoint list is empty + self.waypoints.append(current_point) + else: + # Get the last waypoint + last_wp = self.waypoints[-1] + # Determine directions + last_direction = DSLNode.get_direction(last_wp, current_point) + current_direction = DSLNode.get_direction(current_point, next_point) + + # If there's a change in direction, add the current point to waypoints + if current_direction != last_direction: + #print("Change in direction detected") + self.waypoints.append(current_point) + + def compare_keys(self, key_pair1: tuple[float, float], key_pair2: tuple[float, float]) -> bool: + """ + Compares the priority keys of two nodes. + + Args: + key_pair1 (tuple): The first key pair to compare. + key_pair2 (tuple): The second key pair to compare. + + Returns: + bool: True if `key_pair1` should precede `key_pair2` in sorting order, False otherwise. + """ + return key_pair1[0] < key_pair2[0] or (key_pair1[0] == key_pair2[0] and key_pair1[1] < key_pair2[1]) + + def compute_shortest_path(self): + """ + Computes or recomputes the shortest path from the start to the goal using the D* Lite algorithm. + + This method iteratively updates the priorities and costs of nodes based on the graph's current state, + adjusting the path as necessary until the start DSLNode's key does not precede the smallest key in the + priority queue and the start DSLNode's rhs and g values are equal. + """ + + # Loop until the priority queue is empty, indicating no more nodes to process + while self.priority_queue: + # Sort the priority queue based on the keys to ensure the node with the smallest key is processed first + self.priority_queue.sort(key=lambda x: x[1]) + + # Extract the smallest key and its corresponding node + k_old = self.priority_queue[0][1] + u = self.priority_queue[0][0] + + # 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] + + # Exit the loop if no updates are required + if not start_key_not_updated and not rhs_not_equal_to_g: + break + + # Remove the processed node from the priority queue + self.priority_queue.pop(0) + + # 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) + + def compute_current_path(self) -> list[DSLNode]: + """ + Computes the current path from the start to the goal. + + Returns: + list: A list of DSLNode objects representing the current path from the start to the goal. + """ + 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) + path.append(current_point) + last_point = current_point + current_point = min(self.pred(current_point), key = lambda sprime: self.movement_cost(current_point, sprime) + self.g[sprime.x][sprime.y]) + path.append(self.goal) + self.waypoints.append(self.goal) + + return path + + + def get_WP(self) -> list[Point]: + """ + Retrieves the waypoints and adjusts their coordinates to the original coordinate system. + + Returns: + list: A list of waypoints with their coordinates adjusted to the original coordinate system. + """ + WP_list = [] + for wp in self.waypoints: + WP_list.append(Point(x=float(wp.x), y=float(wp.y), z=0.0)) + return WP_list + + + def dsl_main(self) -> None: + """ + Main function to run the D* Lite algorithm. + + Args: + start (DSLNode): The start DSLNode. + goal (DSLNode): The goal DSLNode. + """ + self.initialize() + self.compute_shortest_path() + self.compute_current_path() + print("Path found") + + \ No newline at end of file diff --git a/guidance/d_star_lite/d_star_lite/d_star_lite_node.py b/guidance/d_star_lite/d_star_lite/d_star_lite_node.py new file mode 100644 index 00000000..1555992e --- /dev/null +++ b/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 + \ No newline at end of file diff --git a/guidance/d_star_lite/d_star_lite/ros_d_star_lite_node.py b/guidance/d_star_lite/d_star_lite/ros_d_star_lite_node.py new file mode 100755 index 00000000..ff03c8f3 --- /dev/null +++ b/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() \ No newline at end of file diff --git a/guidance/d_star_lite/launch/d_star_lite.launch.py b/guidance/d_star_lite/launch/d_star_lite.launch.py new file mode 100644 index 00000000..95ffb0c5 --- /dev/null +++ b/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 + ]) \ No newline at end of file diff --git a/guidance/d_star_lite/package.xml b/guidance/d_star_lite/package.xml new file mode 100644 index 00000000..4a564bba --- /dev/null +++ b/guidance/d_star_lite/package.xml @@ -0,0 +1,30 @@ + + + + d_star_lite + 0.0.0 + d_star_lite algorithm for path planning + andeshog + MIT + + ament_cmake + + ament_lint_auto + ament_lint_common + launch_testing_ament_cmake + launch_testing_ros + + rclpy + std_msgs + vortex_msgs + geometry_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_cmake + + diff --git a/guidance/d_star_lite/resource/D_star_lite b/guidance/d_star_lite/resource/D_star_lite new file mode 100644 index 00000000..e69de29b diff --git a/guidance/d_star_lite/tests/test_d_star_lite.py b/guidance/d_star_lite/tests/test_d_star_lite.py new file mode 100644 index 00000000..5f42d30a --- /dev/null +++ b/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() \ No newline at end of file diff --git a/mission/blackbox/README b/mission/blackbox/README new file mode 100644 index 00000000..23183a8d --- /dev/null +++ b/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 :) \ No newline at end of file diff --git a/mission/blackbox/blackbox/blackbox_log_data.py b/mission/blackbox/blackbox/blackbox_log_data.py index cb4926dd..b4f432a5 100644 --- a/mission/blackbox/blackbox/blackbox_log_data.py +++ b/mission/blackbox/blackbox/blackbox_log_data.py @@ -1,5 +1,6 @@ # Python Libraries import os +import re import time import csv from datetime import datetime, timedelta @@ -69,46 +70,85 @@ def __init__(self, writer.writerow(self.csv_headers) # Methods for inside use of the class ---------- - def manage_csv_files(self, max_file_age_in_days=1, max_size_kb=3_000_000): #adjust the max size before you start deleting old files (1 000 000 kb = 1 000 mb = 1 gb) + def manage_csv_files( + self, + max_file_age_in_days=1, + max_size_kb=3_000_000, + ): #adjust the max size before you start deleting old files (1 000 000 kb = 1 000 mb = 1 gb) current_time = datetime.now() older_than_time = current_time - timedelta(days=max_file_age_in_days) + # Compile a regular expression pattern for matching the expected filename format + pattern = re.compile( + r'blackbox_data_(\d{4}-\d{2}-\d{2}_\d{2}:\d{2}:\d{2})\.csv') + # List all .csv files in the blackbox data directory - csv_files = [f for f in os.listdir(self.blackbox_data_directory) if f.endswith('.csv') and f.startswith('blackbox_data_')] - - # Delete .csv files older than 1 day + csv_files = [ + f for f in os.listdir(self.blackbox_data_directory) + if f.endswith('.csv') and f.startswith('blackbox_data_') + ] + for csv_file in csv_files: - try: - file_time = datetime.strptime(csv_file[13:-4], '%Y-%m-%d_%H:%M:%S') - except ValueError: + match = pattern.match(csv_file) + # Skip files that do not match the expected format + if match is None: print(f'Invalid filename format, skipping file: {csv_file}') continue - + + try: + file_time = datetime.strptime(match.group(1), + '%Y-%m-%d_%H:%M:%S') + except ValueError as e: + print( + f'Error parsing file timestamp, skipping file: {csv_file}. Error: {e}' + ) + continue + if file_time < older_than_time: - file_path = os.path.join(self.blackbox_data_directory, csv_file) + file_path = os.path.join(self.blackbox_data_directory, + csv_file) os.remove(file_path) print(f'Deleted old csv file: {file_path}') # Calculate the total size of remaining .csv files - total_size_kb = sum(os.path.getsize(os.path.join(self.blackbox_data_directory, f)) for f in os.listdir(self.blackbox_data_directory) if f.endswith('.csv')) / 1024 - + total_size_kb = sum( + os.path.getsize(os.path.join(self.blackbox_data_directory, f)) + for f in os.listdir(self.blackbox_data_directory) + if f.endswith('.csv')) / 1024 + + csv_files = [ + f for f in os.listdir(self.blackbox_data_directory) + if f.endswith('.csv') and f.startswith('blackbox_data_') + and pattern.match(f) + ] # Delete oldest files if total size exceeds max_size_kb while total_size_kb > max_size_kb: # Sort .csv files by timestamp (oldest first) - csv_files_sorted = sorted(csv_files, key=lambda x: datetime.strptime(x[13:-4], '%Y-%m-%d_%H:%M:%S')) - + csv_files_sorted = sorted( + csv_files, + key=lambda x: datetime.strptime( + pattern.match(x).group(1), '%Y-%m-%d_%H:%M:%S')) + if not csv_files_sorted: print('No .csv files to delete.') break - + oldest_file = csv_files_sorted[0] - oldest_file_path = os.path.join(self.blackbox_data_directory, oldest_file) + oldest_file_path = os.path.join(self.blackbox_data_directory, + oldest_file) os.remove(oldest_file_path) print(f'Deleted the oldest csv file: {oldest_file_path}') - + # Recalculate the total size of remaining .csv files - total_size_kb = sum(os.path.getsize(os.path.join(self.blackbox_data_directory, f)) for f in os.listdir(self.blackbox_data_directory) if f.endswith('.csv')) / 1024 - print(f'Now the total size of .csv files is: {total_size_kb:.2f} KB') + total_size_kb = sum( + os.path.getsize(os.path.join(self.blackbox_data_directory, f)) + for f in os.listdir(self.blackbox_data_directory) + if f.endswith('.csv')) / 1024 + csv_files.remove( + oldest_file + ) # Ensure the deleted file is removed from the list + print( + f'Now the total size of .csv files is: {total_size_kb:.2f} KB') # Methods for external uses ---------- def log_data_to_csv_file(self, diff --git a/mission/blackbox/startup_scripts/blackbox.service b/mission/blackbox/startup_scripts/blackbox.service index 08f219e9..4733ea95 100644 --- a/mission/blackbox/startup_scripts/blackbox.service +++ b/mission/blackbox/startup_scripts/blackbox.service @@ -4,7 +4,9 @@ After=network.target [Service] # Use the wrapper script for ExecStart -ExecStart=/bin/bash ///src/vortex-asv/mission/blackbox/startup_scripts/blackbox.sh +# DONT CHANGE THIS LINE!!!! +# Use vortex-asv/scripts/add_service_files_to_bootup_sequence.sh to automaticaly set everything up +ExecStart=/bin/bash 'blackbox.sh' WorkingDirectory=/home/vortex/ StandardOutput=journal+console User=vortex diff --git a/mission/internal_status/README.md b/mission/internal_status/README.md index 491c159d..3e8660b0 100644 --- a/mission/internal_status/README.md +++ b/mission/internal_status/README.md @@ -17,6 +17,15 @@ This ros node just publishes the current and voltage data we get from the lib to * /asv/power_sense_module/current for the current data [A] * /asv/power_sense_module/voltage for the voltage data [V] +## Service file bootup + +To start the internal_status publishing automaticaly every time on bootup just run this command: +``` +./vortex-asv/add_service_files_to_bootup_sequence.sh +``` + +Enjoy :) + diff --git a/mission/internal_status/startup_scripts/internal_status.service b/mission/internal_status/startup_scripts/internal_status.service index 4f8feb18..897d3a2c 100644 --- a/mission/internal_status/startup_scripts/internal_status.service +++ b/mission/internal_status/startup_scripts/internal_status.service @@ -4,7 +4,9 @@ After=network.target [Service] # Use the wrapper script for ExecStart -ExecStart=/bin/bash /home/vortex/rose_test_ws/src/vortex-asv/mission/internal_status/startup_scripts/internal_status.sh +# DONT CHANGE THIS LINE!!!! +# Use vortex-asv/scripts/add_service_files_to_bootup_sequence.sh to automaticaly set everything up +ExecStart=/bin/bash 'internal_status.sh' WorkingDirectory=/home/vortex/ StandardOutput=journal+console User=vortex diff --git a/mission/mission_planner/mission_planner/mission_planner.py b/mission/mission_planner/mission_planner/mission_planner.py index 9ca94578..f856fc46 100755 --- a/mission/mission_planner/mission_planner/mission_planner.py +++ b/mission/mission_planner/mission_planner/mission_planner.py @@ -1,6 +1,9 @@ #!/usr/bin/env python3 +<<<<<<< HEAD import sys +======= +>>>>>>> development import rclpy from rclpy.node import Node from vortex_msgs.srv import MissionParameters @@ -52,10 +55,17 @@ def main(args=None): rclpy.init(args=args) mission_planner_client = MissionPlannerClient() # Test data +<<<<<<< HEAD obstacles = [] start = Point(x=1.0, y=1.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) +======= + 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), 30, 30) +>>>>>>> development while rclpy.ok(): rclpy.spin_once(mission_planner_client) diff --git a/scripts/add_service_files_to_bootup_sequence.sh b/scripts/add_service_files_to_bootup_sequence.sh new file mode 100755 index 00000000..22dbbb9d --- /dev/null +++ b/scripts/add_service_files_to_bootup_sequence.sh @@ -0,0 +1,91 @@ +#!/bin/bash + +# Variables ---------- +# Define variables for launching service files +SERVICE_FILE_NAME_BMS="bms.service" +SERVICE_FILE_PATH_BMS="../sensors/bms/startup_scripts/" +SERVICE_FILE_NAME_TEMPERATURE="temperature.service" +SERVICE_FILE_PATH_TEMPERATURE="../sensors/temperature/startup_scripts/" +SERVICE_FILE_NAME_INTERNAL_STATUS="internal_status.service" +SERVICE_FILE_PATH_INTERNAL_STATUS="../mission/internal_status/startup_scripts/" +SERVICE_FILE_NAME_BLACKBOX="blackbox.service" +SERVICE_FILE_PATH_BLACKBOX="../mission/blackbox/startup_scripts/" + +SYSTEMD_PATH="/etc/systemd/system/" + + + +# Navigating ---------- +echo "Navigated to the correct folder..." +# Get scripts directory and go there +SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )/" +cd $SCRIPT_DIR + + + +# Setup ---------- +echo "Setting up .service files..." +# Copy the .service files to current directory +cp $SERVICE_FILE_PATH_BMS$SERVICE_FILE_NAME_BMS $SERVICE_FILE_NAME_BMS # BMS +cp $SERVICE_FILE_PATH_TEMPERATURE$SERVICE_FILE_NAME_TEMPERATURE $SERVICE_FILE_NAME_TEMPERATURE # Temperature +cp $SERVICE_FILE_PATH_INTERNAL_STATUS$SERVICE_FILE_NAME_INTERNAL_STATUS $SERVICE_FILE_NAME_INTERNAL_STATUS # Internal Status +cp $SERVICE_FILE_PATH_BLACKBOX$SERVICE_FILE_NAME_BLACKBOX $SERVICE_FILE_NAME_BLACKBOX # Blackbox + +# Replace placeholders in the .service files +# Note: This assumes is to be replaced with the current directory +sed -i "s||$SCRIPT_DIR$SERVICE_FILE_PATH_BMS|g" $SERVICE_FILE_NAME_BMS # BMS +sed -i "s||$SCRIPT_DIR$SERVICE_FILE_PATH_TEMPERATURE|g" $SERVICE_FILE_NAME_TEMPERATURE # Temperature +sed -i "s||$SCRIPT_DIR$SERVICE_FILE_PATH_INTERNAL_STATUS|g" $SERVICE_FILE_NAME_INTERNAL_STATUS # Internal Status +sed -i "s||$SCRIPT_DIR$SERVICE_FILE_PATH_BLACKBOX|g" $SERVICE_FILE_NAME_BLACKBOX # Blackbox + +# Kill the systems service files if it exists +sudo systemctl kill $SERVICE_FILE_NAME_BMS # BMS +sudo systemctl kill $SERVICE_FILE_NAME_TEMPERATURE # Temperature +sudo systemctl kill $SERVICE_FILE_NAME_INTERNAL_STATUS # Internal Status +sudo systemctl kill $SERVICE_FILE_NAME_BLACKBOX # Blackbox + +# Copy the modified .service files to the systemd directory +# Note: Need sudo permission to copy to /etc/systemd/system +sudo cp $SERVICE_FILE_NAME_BMS $SYSTEMD_PATH # BMS +sudo cp $SERVICE_FILE_NAME_TEMPERATURE $SYSTEMD_PATH # Temperature +sudo cp $SERVICE_FILE_NAME_INTERNAL_STATUS $SYSTEMD_PATH # Internal Status +sudo cp $SERVICE_FILE_NAME_BLACKBOX $SYSTEMD_PATH # Blackbox + +# Delete the redundant copy +rm $SERVICE_FILE_NAME_BMS # BMS +rm $SERVICE_FILE_NAME_TEMPERATURE # Temperature +rm $SERVICE_FILE_NAME_INTERNAL_STATUS # Internal Status +rm $SERVICE_FILE_NAME_BLACKBOX # Blackbox + +# Change permision of the .service files +sudo chmod 777 $SYSTEMD_PATH$SERVICE_FILE_NAME_BMS # BMS +sudo chmod 777 $SYSTEMD_PATH$SERVICE_FILE_NAME_TEMPERATURE # Temperature +sudo chmod 777 $SYSTEMD_PATH$SERVICE_FILE_NAME_INTERNAL_STATUS # Internal Status +sudo chmod 777 $SYSTEMD_PATH$SERVICE_FILE_NAME_BLACKBOX # Blackbox + + + +# Launching ---------- +echo "Launching .service files..." +# Reload systemd to recognize the new services +sudo systemctl daemon-reload + +# Enable new services to start on boot +sudo systemctl enable $SERVICE_FILE_NAME_BMS # BMS +sudo systemctl enable $SERVICE_FILE_NAME_TEMPERATURE # Temperature +sudo systemctl enable $SERVICE_FILE_NAME_INTERNAL_STATUS # Internal Status +sudo systemctl enable $SERVICE_FILE_NAME_BLACKBOX # Blackbox + +# Start the services immediately +sudo systemctl start $SERVICE_FILE_NAME_BMS # BMS +sudo systemctl start $SERVICE_FILE_NAME_TEMPERATURE # Temperature +sudo systemctl start $SERVICE_FILE_NAME_INTERNAL_STATUS # Internal Status +sudo systemctl start $SERVICE_FILE_NAME_BLACKBOX # Blackbox + + + +# Debugging ---------- +echo "'$SERVICE_FILE_NAME_BMS' - installed and started successfully :)" +echo "'$SERVICE_FILE_NAME_TEMPERATURE' - installed and started successfully :)" +echo "'$SERVICE_FILE_NAME_INTERNAL_STATUS' - installed and started successfully :)" +echo "'$SERVICE_FILE_NAME_BLACKBOX' - installed and started successfully :)" \ No newline at end of file diff --git a/sensors/bms/readme.md b/sensors/bms/readme.md index 355769a3..3aad312a 100644 --- a/sensors/bms/readme.md +++ b/sensors/bms/readme.md @@ -82,27 +82,12 @@ create_key_value_pair(key: str, value) -> KeyValue: creates KeyValue object from supplied key and value ``` -## Startup service +## Service file bootup -Note: The .service file may need to be edited in order to source the setup.bash file -from the correct workspace. - -This package also contains a .service file for running the node on startup. To enable this, just run (from this directory) -``` -sudo cp startup_script/bms_startup.service /etc/systemd/system/bms_startup.service -``` -then run the following commands -``` -cd /etc/systemd/system -``` -``` -sudo systemctl daemon-reload && sudo systemctl enable bms_startup.service -``` -To verify that the service is enabled, run +To start the BMS publishing automaticaly every time on bootup just run this command: ``` -systemctl list-unit-files | grep enabled +./vortex-asv/add_service_files_to_bootup_sequence.sh ``` -The file will now run automatically on startup the next time the system starts. -If this fails, try checking if systemd is installed. +Enjoy :) diff --git a/sensors/bms/startup_script/bms.service b/sensors/bms/startup_script/bms.service deleted file mode 100644 index b5c91959..00000000 --- a/sensors/bms/startup_script/bms.service +++ /dev/null @@ -1,17 +0,0 @@ -[Unit] -Description=launch ros2 bms node -After=network.target - -[Service] -ExecStart=/bin/bash /home///src/vortex-asv/sensors/bms/startup_script/bms.sh -WorkingDirectory=/home// -RemainAfterExit=yes -StandardOutput=journal+console - -User=vortex - -Restart=no -RestartSec=2 - -[Install] -WantedBy=multi-user.target diff --git a/sensors/bms/startup_script/bms.sh b/sensors/bms/startup_script/bms.sh deleted file mode 100755 index 429d803c..00000000 --- a/sensors/bms/startup_script/bms.sh +++ /dev/null @@ -1,7 +0,0 @@ -# Determine the directory where the script is located -SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )" - -cd $SCRIPT_DIR -cd ../../../../.. # Go back to workspace -source install/setup.bash -ros2 launch bms bms_launch.py \ No newline at end of file diff --git a/sensors/bms/startup_scripts/bms_startup.service b/sensors/bms/startup_scripts/bms.service similarity index 59% rename from sensors/bms/startup_scripts/bms_startup.service rename to sensors/bms/startup_scripts/bms.service index 513e979b..34a7953d 100644 --- a/sensors/bms/startup_scripts/bms_startup.service +++ b/sensors/bms/startup_scripts/bms.service @@ -4,7 +4,9 @@ After=network.target [Service] # Use the wrapper script for ExecStart -ExecStart=/bin/bash ///src/vortex-asv/sensors/bms/startup_scripts/bms.sh +# DONT CHANGE THIS LINE!!!! +# Use vortex-asv/scripts/add_service_files_to_bootup_sequence.sh to automaticaly set everything up +ExecStart=/bin/bash 'bms.sh' WorkingDirectory=/home/vortex/ StandardOutput=journal+console User=vortex diff --git a/sensors/bms/startup_scripts/bms_startup_backup.service b/sensors/bms/startup_scripts/bms_startup_backup.service deleted file mode 100644 index 53f33a25..00000000 --- a/sensors/bms/startup_scripts/bms_startup_backup.service +++ /dev/null @@ -1,23 +0,0 @@ -[Unit] -Description=launch ros2 bms node -After=network.target - -[Service] -ExecStart=/bin/bash -c 'source /opt/ros/humble/setup.bash; source /usr/share/colcon_argcomplete/hook/colcon-argcomplete.bash; source /usr/share/colcon_cd/function/colcon_cd.sh colcon build; source /home/vortex/david_test_ws/install/setup.bash; ros2 launch bms bms_launch.py; sleep 60' -RemainAfterExit=yes -WorkingDirectory=/home/vortex/david_test_ws -StandardOutput=journal+console - - -Environment="HOME=root" -Environment="ROS_DOMAIN_ID=1" -Environment="ROS_LOCALHOST_ONLY=1" -Environment="_colcon_cd_root=/top/ros/rolling/" - -User=vortex - -Restart=no -RestartSec=2 - -[Install] -WantedBy=multi-user.target diff --git a/sensors/temperature/README b/sensors/temperature/README new file mode 100644 index 00000000..8ad1b40e --- /dev/null +++ b/sensors/temperature/README @@ -0,0 +1,11 @@ +## Temperature + +We messure temperature of the 4 ESCs that run the thrusters as well as 2 temperure sensors for the ambient temperature in the electrical housing + +## Service file bootup + +To start the Temperature publishing automaticaly every time on bootup just run this command: +./vortex-asv/add_service_files_to_bootup_sequence.sh + +Enjoy :) + diff --git a/sensors/temperature/startup_scripts/temperature.service b/sensors/temperature/startup_scripts/temperature.service index 39b0e36d..a1b44d36 100644 --- a/sensors/temperature/startup_scripts/temperature.service +++ b/sensors/temperature/startup_scripts/temperature.service @@ -4,7 +4,9 @@ After=network.target [Service] # Use the wrapper script for ExecStart -ExecStart=/bin/bash /home/vortex/rose_test_ws/src/vortex-asv/sensors/temperature/startup_scripts/temperature.sh +# DONT CHANGE THIS LINE!!!! +# Use vortex-asv/scripts/add_service_files_to_bootup_sequence.sh to automaticaly set everything up +ExecStart=/bin/bash 'temperature.sh' WorkingDirectory=/home/vortex/ StandardOutput=journal+console User=vortex