diff --git a/asv_setup/config/robots/freya.yaml b/asv_setup/config/robots/freya.yaml index cc960e68..19c59c15 100644 --- a/asv_setup/config/robots/freya.yaml +++ b/asv_setup/config/robots/freya.yaml @@ -21,6 +21,8 @@ buoyancy: 0 # kg center_of_mass: [0, 0, 0] # mm (x,y,z) center_of_buoyancy: [0, 0, 0] # mm (x,y,z) + inertia_matrix: [90.5, 0.0, 0.0, 0.0, 167.5, 12.25, 0.0, 12.25, 42.65] # from otter + damping_matrix_diag: [77.55, 162.5, 42.65] # from otter propulsion: diff --git a/asv_setup/launch/hybridpath.launch.py b/asv_setup/launch/hybridpath.launch.py new file mode 100644 index 00000000..aef40237 --- /dev/null +++ b/asv_setup/launch/hybridpath.launch.py @@ -0,0 +1,25 @@ +import os +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.actions import SetEnvironmentVariable, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from ament_index_python.packages import get_package_share_directory + +def generate_launch_description(): + hybridpath_controller_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory('hybridpath_controller'), 'launch', 'hybridpath_controller.launch.py') + ) + ) + + hybridpath_guidance_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory('hybridpath_guidance'), 'launch', 'hybridpath_guidance.launch.py') + ) + ) + + # Return launch description + return LaunchDescription([ + hybridpath_controller_launch, + hybridpath_guidance_launch + ]) \ No newline at end of file diff --git a/guidance/hybridpath_guidance/CMakeLists.txt b/guidance/hybridpath_guidance/CMakeLists.txt new file mode 100644 index 00000000..0fca6d21 --- /dev/null +++ b/guidance/hybridpath_guidance/CMakeLists.txt @@ -0,0 +1,35 @@ +cmake_minimum_required(VERSION 3.8) +project(hybridpath_guidance) + +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(vortex_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) + +ament_python_install_package(${PROJECT_NAME}) + +install(DIRECTORY + launch + config + DESTINATION share/${PROJECT_NAME} +) + +install(PROGRAMS + hybridpath_guidance/hybridpath_guidance_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/hybridpath_guidance/README.md b/guidance/hybridpath_guidance/README.md new file mode 100755 index 00000000..3a512865 --- /dev/null +++ b/guidance/hybridpath_guidance/README.md @@ -0,0 +1,131 @@ +# Hybrid Path Guidance + +This package provides the implementation of hybrid path guidance for the Vortex ASV. + +## Usage + +To use the hybrid path guidance launch it using: `ros2 launch hybridpath_guidance hybridpath_guidance.launch` +Or alternatively, run it together with the hybridpath controller using the launch file `hybridpath.launch.py` in asv_setup + +To run with custom waypoints (replace example waypoints with actual waypoints, and add as many prefered): + +`ros2 service call waypoint_list vortex_msgs/srv/Waypoint "waypoint: [{x: 0.0, y: 0.0, z: 0.0}, {x: 5.0, y: 5.0, z: 0.0}]"` + +## Configuration + +You can configure the behavior of the hybrid path guidance by modifying the parameters in the `config` directory. + +## Theory +### Path generation +Given a set of $n + 1$ waypoints, the hybridpath generator will generate a $C^r$ path that goes through the waypoints. That is, the path is continuous in $r$ derivatives. Towards constructing the overall desired path $p_d(s)$, it is first divided into $n$ subpaths $p_{d,i}(s)$ for $i = 1, ... , n$ between the waypoints. Each of these is expressed as a polynomial in $s$ of a certain order. Then the expressions for the subpaths are concatenated at the waypoints to assemble the full path. The order of the polynomials must be chosen sufficiently high to ensure the overall path is sufficiently differentiable at the waypoints. Increasing the order of the polynomial increases the number of coefficients giving more degrees-of-freedom to satisfy these continuity constraints. + +The variable $s \in [0,n)$ is called the path variable, and it tells us exactly where we are on the path. For example, $s$ will be $0$ at the first waypoint, $1$ at the second, and so on. However, using the path parameter to take values $s \in [0,n)$ is unnecessary and can be numerically problematic. Instead, we can identify each path segment $p_i$ and parametrize it by $\theta \in [0,1)$. We define +```math +i := \lfloor s \rfloor + 1 +``` +```math +\theta = s - \lfloor s \rfloor \in [0,1) +``` + +to get the continuous map +```math +s \mapsto p_d(s) +``` + +Consider polynomials of order $k$, +```math +x_{d,i}(\theta) = a_{k,i} \theta^k + ... + a_{1,i} \theta + a_{0,i} +``` +```math + y_{d,i}(\theta) = b_{k,i} \theta^k + ... + b_{1,i} \theta + b_{0,i} +``` +where the coefficients {$`a_{j,i}`$, $`b_{j,i}`$} must be determined. For each subpath there are $(k + 1)$ $\cdot$ $2$ unknowns so that there are $(k + 1)$ $\cdot$ $2n$ unknown coefficients in total to be determined for the full path. This is done by setting up and solving the linear system +```math +A\phi = b \; \; \; \; \; \; \phi^T = [a^T, b^T] +``` +for each path segment and solve them in a single operation as $` \phi = A^{-1}b`$. + +Assuming that the subpaths is to be connected at the waypoints, we use the following procedure to calculate the coefficients of each subpath: + +$C^0$ : Continuity at the waypoints gives for i: +```math +x_{d,i}(0) = x_{i-1} \; \; \; \; \; \; y_{d,i}(0) = y_{i-1} +``` +```math + x_{d,i}(1) = x_i \; \; \; \; \; \; y_{d,i}(1) = y_i +``` + +$C^1$ : The slopes at the first and last waypoints are chosen as: +```math +x^\theta_{d,i}(0) = x_1 - x_0 \; \; \; \; \; \; y^\theta_{d,1}(0) = y_1 - y_0 +``` +```math + x^\theta_{d,n}(1) = x_n - x_{n-1} \; \; \; \; \; \; y^\theta_{d,n}(1) = y_n - y_{n-1} +``` + +Here, $x^\theta$ represents the derivative of $x$ with respect to $\theta$. + +The slopes at the intermediate waypoints are chosen as: +```math + x^\theta_{d,i}(0) = \lambda (x_i - x_{i-2}) \; \; \; \; \; \; i = 2, ... , n +``` +```math + y^\theta_{d,i}(0) = \lambda (y_i - y_{i-2}) \; \; \; \; \; \; i = 2, ... , n +``` +```math + x^\theta_{d,i}(1) = \lambda (x_{i+1} - x_{i-1}) \; \; \; \; \; \; i = 1, ... , n-1 +``` +```math + y^\theta_{d,1}(1) = \lambda (y_{i+1} - y_{i-1}) \; \; \; \; \; \; i = 1, ... , n-1 +``` +where $\lambda > 0$ is a design constant. Choosing $\lambda > 0.5$ gives more rounded corners, choosing $\lambda < 0.5$ gives sharper corners and choosing $\lambda = 0.5$ makes the slope at waypoint $p_i$ be the average of pointing to $p_{i-1}$ and $p_{i+1}$ + +$C^j$ : Setting derivatives of order $j = 2, 3, ... , k$ to zero for i: +```math + x^{\theta^j}_{d,i}(0) = 0 \; \; \; \; \; \; y^{\theta^j}_{d,i}(0) = 0 +``` +```math +x^{\theta^j}_{d,i}(1) = 0 \; \; \; \; \; \; y^{\theta^j}_{d,i}(1) = 0 +``` +This results in the hybrid parametrization of the path, +```math +\bar{p_d}(i,\theta) = \begin{bmatrix} x_{d,i}(\theta) \\ y_{d,i}(\theta) \end{bmatrix} +``` + +where $\theta \in [0,1)$. + +If the differentiability requirement of the path is $C^r$, then the above equations up to $j=r$ gives $2(r+1) \cdot 2n$ equations to solve for $(k + 1) \cdot 2n$ unknown coefficients. As a result, the order $k$ of the polynomials must be +```math +k = 2r + 1 +``` + +### Guidance system +So far we have defined the desired path +```math +p_d(i,\theta) = \begin{bmatrix} x_d(i,\theta) \\ y_d(i,\theta) \end{bmatrix},\; \theta \in [0,1) +``` +. Now we define the desired heading +```math +\psi_d = atan2(y^\theta_d(i,\theta), x^\theta_d(i,\theta)) +``` +where $atan2(y,x)$ is the four-quadrant version of $arctan(y/x)$. As mentioned previously, the pair $(i,\theta)$ tells us which path segment $i$ we are on and where on the segment we are (given by $\theta$). Since we also need the first and second derivatives of the heading for the controller, we define them as: +```math +\psi^\theta_d(i,\theta) = \frac{x^\theta_d(i,\theta)y^{\theta^2}_d(i,\theta)-x^{\theta^2}_d(i,\theta)y^\theta_d(i,\theta)}{x^\theta_d(i,\theta)^2+y^\theta_d(i,\theta)^2} +``` + +Differentiating one more time yields: + +```math +\psi^{\theta^2}_d(i,\theta) = \frac{x^\theta_d(i,\theta)y^{\theta^3}_d(i,\theta)-x^{\theta^3}_d(i,\theta)y^\theta_d(i,\theta)}{x^\theta_d(i,\theta)^2+y^\theta_d(i,\theta)^2} \\-2\frac{(x^\theta_d(i,\theta)y^{\theta^2}_d(i,\theta)-x^{\theta^2}_d(i,\theta)y^\theta_d(i,\theta))(x^\theta_d(i,\theta)x^{\theta^2}_d(i,\theta)-y^{\theta^2}_d(i,\theta)y^\theta_d(i,\theta))}{(x^\theta_d(i,\theta)^2+y^\theta_d(i,\theta)^2)^2} +``` + +Source: https://folk.ntnu.no/rskjetne/Publications/SkjetnePhDthesis_B5_compressed.pdf + +### Example plots + +These plots showcases the what the $r$ and $\lambda$ parameters does to the path. + +![Path Plots](https://drive.google.com/uc?export=download&id=1CE8kN1yJSjEgiCdWGQYgjdIfSkE40V1f) + +![Lambda Plots](https://drive.google.com/uc?export=download&id=1TtpIAwBRKcZhpuXAUEinpuqUhN-WKoX-) + diff --git a/guidance/hybridpath_guidance/config/hybridpath_guidance_config.yaml b/guidance/hybridpath_guidance/config/hybridpath_guidance_config.yaml new file mode 100644 index 00000000..1ca011e7 --- /dev/null +++ b/guidance/hybridpath_guidance/config/hybridpath_guidance_config.yaml @@ -0,0 +1,9 @@ +/**: + ros__parameters: + hybridpath_guidance: + lambda_val: 0.15 # Curvature constant + path_generator_order: 1 # Differentiability order + time_to_max_speed: 10.0 # Time to reach maximum speed + dt: 0.1 # Time step + u_desired: 0.5 # Desired speed + mu: 0.03 # Tuning parameter \ No newline at end of file diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/__init__.py b/guidance/hybridpath_guidance/hybridpath_guidance/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath.py b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath.py new file mode 100644 index 00000000..f6787cbd --- /dev/null +++ b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath.py @@ -0,0 +1,426 @@ +from __future__ import annotations +from dataclasses import dataclass, field +import numpy as np +from geometry_msgs.msg import Point + +@dataclass +class LinSys: + """ + A class to represent the linear system. + + Attributes: + A: The coefficients matrix. + bx: The x-vector for each subpath. + by: The y-vector for each subpath. + """ + A: np.ndarray = None + bx: list[np.ndarray] = field(default_factory=list) + by: list[np.ndarray] = field(default_factory=list) + +@dataclass +class Coeff: + """ + A class to represent the coefficients. + + Attributes: + a: The x-coefficients. + b: The y-coefficients. + a_der: The x-coefficients for derivatives. + b_der: The y-coefficients for derivatives. + """ + a: list[np.ndarray] = field(default_factory=list) + b: list[np.ndarray] = field(default_factory=list) + a_der: list[list[np.ndarray]] = field(default_factory=list) + b_der: list[list[np.ndarray]] = field(default_factory=list) + +@dataclass +class Path: + """ + A class to represent the path. + + Attributes: + NumSubpaths (int): The number of subpaths. + Order (int): The order of the subpaths. + LinSys (LinSys): The linear system to solve the subpaths. + coeff (Coeff): The coefficients. + """ + NumSubpaths: int = 0 + Order: int = 0 + LinSys: LinSys = LinSys() + coeff: Coeff = Coeff() + +class HybridPathGenerator: + """ + This class generates a path that goes through a set of waypoints. + + Args: + WP (list[Point]): A list of waypoints. + r (int): The differentiability order. + lambda_val (float): The curvature constant. + + Attributes: + WP (np.ndarray): The waypoints. + r (int): The differentiability order. + lambda_val (float): The curvature constant. + order (int): The order of the subpaths. + path (Path): The path object. + """ + def __init__(self, WP: list[Point], r: int, lambda_val: float): + WP_arr = np.array([[int(wp.x), int(wp.y)] for wp in WP]) + if len(WP_arr) == 2: + self.WP = np.array([WP_arr[0], [(WP_arr[0][0] + WP_arr[1][0])/2, (WP_arr[0][1] + WP_arr[1][1])/2], WP_arr[1]]) + else: + self.WP = WP_arr + self.r = r + self.lambda_val = lambda_val + self.order = 2*r + 1 + self.path = Path() + self.path.NumSubpaths = len(self.WP) - 1 + self.path.Order = self.order + self._calculate_subpaths() + + def _construct_A_matrix(self) -> np.ndarray: + """ + Constructs and returns the A matrix. + + Returns: + np.ndarray: The A matrix. + """ + ord_plus_one = self.order + 1 + A = np.zeros((ord_plus_one, ord_plus_one)) + coefs = np.ones(ord_plus_one) + A[0,0] = coefs[0] + A[1,:] = coefs + c_der = coefs.copy() + for k in range(2, (self.order + 1) // 2 + 1): + c_der = np.polyder(c_der[::-1])[::-1] + coefs = np.concatenate([np.zeros(k-1), c_der]) + A[2*k-2, k-1] = c_der[0] + A[2*k-1, :] = coefs + return A + + def _calculate_subpath_coeffs(self, j: int) -> tuple[np.ndarray, np.ndarray]: + """ + Calculate the subpath coefficients for a given index. + + Parameters: + j (int): The index of the subpath. + + Returns: + tuple[np.ndarray, np.ndarray]: A tuple containing two numpy arrays representing the subpath coefficients. + """ + order_plus_one = self.order + 1 + N = self.path.NumSubpaths + ax, bx = np.zeros(order_plus_one), np.zeros(order_plus_one) + ax[:2] = self.WP[j:j+2, 0] + bx[:2] = self.WP[j:j+2, 1] + + if self.order > 2: + self._calculate_subpaths_coeffs_if_ord_greater_than_2(ax, bx, j, N) + + return ax, bx + + def _calculate_subpaths_coeffs_if_ord_greater_than_2(self, ax: np.ndarray, bx: np.ndarray, j: int, N: int) -> None: + """ + Calculate the coefficients for subpaths when the order is greater than 2. + See the README for the details. + + Args: + ax (np.ndarray): Array to store the x-coefficients. + bx (np.ndarray): Array to store the y-coefficients. + j (int): Index of the current subpath. + N (int): Total number of subpaths. + + Returns: + None + """ + if j == 0: + ax[2:4] = [self.WP[j+1, 0] - self.WP[j, 0], self.lambda_val * (self.WP[j+2, 0] - self.WP[j, 0])] + bx[2:4] = [self.WP[j+1, 1] - self.WP[j, 1], self.lambda_val * (self.WP[j+2, 1] - self.WP[j, 1])] + + elif j == N - 1: + ax[2:4] = [self.lambda_val * (self.WP[j+1, 0] - self.WP[j-1, 0]), self.WP[j+1, 0] - self.WP[j, 0]] + bx[2:4] = [self.lambda_val * (self.WP[j+1, 1] - self.WP[j-1, 1]), self.WP[j+1, 1] - self.WP[j, 1]] + + else: + ax[2:4] = [self.lambda_val * (self.WP[j+1, 0] - self.WP[j-1, 0]), self.lambda_val * (self.WP[j+2, 0] - self.WP[j, 0])] + bx[2:4] = [self.lambda_val * (self.WP[j+1, 1] - self.WP[j-1, 1]), self.lambda_val * (self.WP[j+2, 1] - self.WP[j, 1])] + + def solve_linear_system(self, A: np.ndarray, ax: np.ndarray, bx: np.ndarray) -> tuple[np.ndarray, np.ndarray]: + """ + Solves the linear system for the given subpath. + + Args: + A (np.ndarray): The A matrix. + ax (np.ndarray): The x-vector for the subpath. + bx (np.ndarray): The y-vector for the subpath. + + Returns: + tuple[np.ndarray, np.ndarray]: The coefficients a and b for the subpath. + """ + a = np.linalg.solve(A, ax) + b = np.linalg.solve(A, bx) + return a, b + + def _calculate_derivatives(self, vec: np.ndarray) -> np.ndarray: + """ + Calculate the derivatives of a given vector. + + Parameters: + vec (np.ndarray): The input vector. + + Returns: + np.ndarray: A list of arrays representing the derivatives of the input vector. + """ + derivatives = [] + for _ in range(1, self.order + 1): + vec = np.polyder(vec[::-1])[::-1] + derivatives.append(vec) + return derivatives + + def _append_derivatives(self, k: int, a: np.ndarray, b: np.ndarray) -> None: + """ + Append derivatives of coefficients 'a' and 'b' to the path object. + + Parameters: + k (int): The index of the derivative. + a (np.ndarray): The derivative of coefficient 'a'. + b (np.ndarray): The derivative of coefficient 'b'. + + Returns: + None + """ + if k < len(self.path.coeff.a_der): + self.path.coeff.a_der[k].append(a) + self.path.coeff.b_der[k].append(b) + else: + self.path.coeff.a_der.append([a]) + self.path.coeff.b_der.append([b]) + + def _calculate_subpaths(self) -> None: + """ + Calculates the subpaths of the hybrid path. + + This method constructs the A matrix, sets it as the A matrix of the path's linear system, + and calculates the coefficients for each subpath. It then solves the linear system for each + subpath to obtain the coefficients a and b. The derivatives of a and b are also calculated + and appended to the path's derivatives. + + Returns: + None + """ + + A = self._construct_A_matrix() + self.path.LinSys.A = A + + N = self.path.NumSubpaths + for j in range(N): + ax, bx = self._calculate_subpath_coeffs(j) + self.path.LinSys.bx.append(ax) + self.path.LinSys.by.append(bx) + + a_vec, b_vec = self.solve_linear_system(A, ax, bx) + self.path.coeff.a.append(a_vec) + self.path.coeff.b.append(b_vec) + + a_derivatives = self._calculate_derivatives(a_vec) + b_derivatives = self._calculate_derivatives(b_vec) + + for k, (a, b) in enumerate(zip(a_derivatives, b_derivatives)): + self._append_derivatives(k, a, b) + + @staticmethod + def update_s(path: Path, dt: float, u_desired: float, s: float, w: float) -> float: + """ + Update the position along the hybrid path based on the desired velocity and time step. + + Args: + path (Path): The hybrid path. + dt (float): The time step. + u_desired (float): The desired velocity. + s (float): The current position along the hybrid path. + + Returns: + float: The updated position along the hybrid path. + + """ + signals = HybridPathSignals(path, s) + v_s = signals.get_vs(u_desired) + s_new = s + (v_s + w) * dt + return s_new + + +class HybridPathSignals: + """ + Given a path and the path parameter s, this class can get the position + and derivatives of the path at the given point s. + + Args: + Path (Path): The path object. + s (float): The path parameter. + + Attributes: + Path (Path): The path object. + s (float): The path parameter. + """ + def __init__(self, path: Path, s: float): + if not isinstance(path, Path): + raise TypeError("path must be an instance of Path") + self.path = path + self.s = self._clamp_s(s, self.path.NumSubpaths) + + def _clamp_s(self, s: float, num_subpaths: int) -> float: + """ + Ensures s is within the valid range of [0, num_subpaths]. + + Args: + s (float): The path parameter. + num_subpaths (int): The number of subpaths. + + Returns: + float: The clamped path parameter. + """ + if s < 0: + return 0.0 + elif s >= num_subpaths: + return num_subpaths - 1e-3 + return s + + + def get_position(self) -> list[float]: + """ + Get the position of the object along the path. + + Returns: + list[float]: The x and y coordinates of the object's position. + """ + index = int(self.s) + 1 + theta = self.s - (index - 1) + theta_pow = theta ** np.arange(self.path.Order + 1) + a = self.path.coeff.a[index - 1] + b = self.path.coeff.b[index - 1] + x_pos = np.dot(a, theta_pow) + y_pos = np.dot(b, theta_pow) + + return [x_pos, y_pos] + + def _compute_derivatives(self, theta: float, a_vec: np.ndarray, b_vec: np.ndarray) -> list[float]: + """ + Compute the derivatives of x and y. + + Parameters: + theta (float): The value of theta. + a_vec (np.ndarray): The vector of coefficients for x. + b_vec (np.ndarray): The vector of coefficients for y. + + Returns: + list[float]: A list containing the derivatives of x and y. + + """ + theta_pow = theta ** np.arange(len(a_vec)) + x_der = np.dot(a_vec, theta_pow) + y_der = np.dot(b_vec, theta_pow) + return [x_der, y_der] + + def get_derivatives(self) -> list[list[float]]: + """ + Get the derivatives of the path at the current position. + + Returns: + list: A list of computed derivatives. + """ + derivatives = [] + index = int(self.s) + 1 + theta = self.s - (index - 1) + for k in range(1, self.path.Order + 1): + a_vec = self.path.coeff.a_der[k - 1][index - 1] + b_vec = self.path.coeff.b_der[k - 1][index - 1] + derivatives.append(self._compute_derivatives(theta, a_vec, b_vec)) + + return derivatives + + def get_heading(self) -> float: + """ + Get the heading of the object along the path. + + Returns: + float: The heading of the object. + """ + p_der = self.get_derivatives()[0] + psi = np.arctan2(p_der[1], p_der[0]) + return psi + + def get_heading_derivative(self) -> float: + """ + Get the derivative of the heading of the object along the path. + + Returns: + float: The derivative of the heading. + """ + p_der = self.get_derivatives()[0] + p_dder = self.get_derivatives()[1] + psi_der = (p_der[0] * p_dder[1] - p_der[1] * p_dder[0]) / (p_der[0]**2 + p_der[1]**2) + return psi_der + + def get_heading_second_derivative(self) -> float: + """ + Get the second derivative of the heading of the object along the path. + + Returns: + float: The second derivative of the heading. + """ + p_der = self.get_derivatives()[0] + p_dder = self.get_derivatives()[1] + p_ddder = self.get_derivatives()[2] + psi_dder = (p_der[0] * p_ddder[1] - p_der[1] * p_ddder[0]) / (p_der[0]**2 + p_der[1]**2) - 2 * (p_der[0] * p_dder[1] - p_dder[0] * p_der[1]) * (p_der[0] * p_dder[0] - p_dder[1] * p_der[0]) / ((p_der[0]**2 + p_der[1]**2)**2) + return psi_dder + + def get_vs(self, u_desired: float) -> float: + """ + Calculate the reference velocity. + + Args: + u_desired (float): The desired velocity. + + Returns: + float: The reference velocity. + """ + p_der = self.get_derivatives()[0] + v_s = u_desired / np.linalg.norm(p_der) + return v_s + + def get_vs_derivative(self, u_desired: float) -> float: + """ + Calculate the derivative of the reference velocity. + + Args: + u_desired (float): The desired velocity. + + Returns: + float: The derivative of the reference velocity. + """ + p_der = self.get_derivatives()[0] + p_dder = self.get_derivatives()[1] + v_s_s = -u_desired * (np.dot(p_der, p_dder)) / (np.sqrt(p_der[0]**2 + p_der[1]**2)**3) + return v_s_s + + def get_w(self, mu: float, eta: np.ndarray) -> float: + """ + Calculates and returns the value of the unit-tangent gradient update law based on the given parameters. + See chapter 3.4 in https://folk.ntnu.no/rskjetne/Publications/SkjetnePhDthesis_B5_compressed.pdf for more details. + + Parameters: + mu (float): A tuning value. + eta (np.ndarray): An array representing the position and heading of the vessel. + + Returns: + float: The calculated value of w. + """ + eta_d = np.array([self.get_position()[0], self.get_position()[1], self.get_heading()]) + error = eta - eta_d + p_der = self.get_derivatives()[0] + psi_der = self.get_heading_derivative() + eta_d_s = np.array([p_der[0], p_der[1], psi_der]) + w = (mu / np.linalg.norm(eta_d_s)) * np.dot(eta_d_s, error) + return w + \ No newline at end of file diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py new file mode 100755 index 00000000..d3c9b678 --- /dev/null +++ b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py @@ -0,0 +1,130 @@ +#!/usr/bin/env python3 +import numpy as np +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import Pose2D +from vortex_msgs.msg import HybridpathReference +from vortex_msgs.srv import Waypoint +from nav_msgs.msg import Odometry +from transforms3d.euler import quat2euler + +from hybridpath_guidance.hybridpath import HybridPathGenerator, HybridPathSignals + +class Guidance(Node): + def __init__(self): + super().__init__("hp_guidance") + self.declare_parameters( + namespace='', + parameters=[ + ('hybridpath_guidance.lambda_val', 0.15), + ('hybridpath_guidance.path_generator_order', 1), + ('hybridpath_guidance.time_to_max_speed', 10.0), + ('hybridpath_guidance.dt', 0.1), + ('hybridpath_guidance.u_desired', 0.5), + ('hybridpath_guidance.mu', 0.03) + ]) + + self.waypoint_server = self.create_service(Waypoint, 'waypoint_list', self.waypoint_callback) + self.eta_subscriber_ = self.state_subscriber_ = self.create_subscription(Odometry, '/sensor/seapath/odom/ned', self.eta_callback, 1) + self.guidance_publisher = self.create_publisher(HybridpathReference, 'guidance/hybridpath/reference', 1) + + # Get parameters + self.lambda_val = self.get_parameter('hybridpath_guidance.lambda_val').get_parameter_value().double_value + self.path_generator_order = self.get_parameter('hybridpath_guidance.path_generator_order').get_parameter_value().integer_value + self.dt = self.get_parameter('hybridpath_guidance.dt').get_parameter_value().double_value + self.u_desired = self.get_parameter('hybridpath_guidance.u_desired').get_parameter_value().double_value + self.mu = self.get_parameter('hybridpath_guidance.mu').get_parameter_value().double_value + self.eta = np.zeros(3) + + # Flags for logging + self.waypoints_received = False + self.waiting_message_printed = False + + # Timer for guidance + timer_period = 0.1 + self.timer = self.create_timer(timer_period, self.guidance_callback) + + def waypoint_callback(self, request, response): + self.get_logger().info('Received waypoints, generating path...') + self.waypoints = request.waypoint + generator = HybridPathGenerator(self.waypoints, self.path_generator_order, self.lambda_val) + self.path = generator.path + self.waypoints_received = True + self.waiting_message_printed = False # Reset this flag to handle multiple waypoint sets + self.s = 0 + signals = HybridPathSignals(self.path, self.s) + self.w = signals.get_w(self.mu, self.eta) + response.success = True + return response + + def eta_callback(self, msg: Odometry): + self.eta = self.odom_to_eta(msg) + + def guidance_callback(self): + if self.waypoints_received: + self.s = HybridPathGenerator.update_s(self.path, self.dt, self.u_desired, self.s, self.w) + signals = HybridPathSignals(self.path, self.s) + self.w = signals.get_w(self.mu, self.eta) + + pos = signals.get_position() + pos_der = signals.get_derivatives()[0] + pos_dder = signals.get_derivatives()[1] + + psi = signals.get_heading() + psi_der = signals.get_heading_derivative() + psi_dder = signals.get_heading_second_derivative() + + hp_msg = HybridpathReference() + hp_msg.eta_d = Pose2D(x=pos[0], y=pos[1], theta=psi) + hp_msg.eta_d_s = Pose2D(x=pos_der[0], y=pos_der[1], theta=psi_der) + hp_msg.eta_d_ss = Pose2D(x=pos_dder[0], y=pos_dder[1], theta=psi_dder) + + hp_msg.w = signals.get_w(self.mu, self.eta) + hp_msg.v_s = signals.get_vs(self.u_desired) + hp_msg.v_ss = signals.get_vs_derivative(self.u_desired) + + self.guidance_publisher.publish(hp_msg) + + if self.s >= self.path.NumSubpaths: + self.waypoints_received = False + self.waiting_message_printed = False + self.get_logger().info('Last waypoint reached, s = %f' % self.s) + + else: + if not self.waiting_message_printed: + self.get_logger().info('Waiting for waypoints to be received') + self.waiting_message_printed = True + + @staticmethod + def odom_to_eta(msg: Odometry) -> np.ndarray: + """ + Converts an Odometry message to 3DOF eta vector. + + Args: + msg (Odometry): The Odometry message to convert. + + Returns: + np.ndarray: The eta vector. + """ + x = msg.pose.pose.position.x + y = msg.pose.pose.position.y + orientation_q = msg.pose.pose.orientation + orientation_list = [ + orientation_q.w, orientation_q.x, orientation_q.y, orientation_q.z + ] + + # Convert quaternion to Euler angles + yaw = quat2euler(orientation_list)[2] + + state = np.array([x, y, yaw]) + return state + +def main(args=None): + rclpy.init(args=args) + guidance = Guidance() + rclpy.spin(guidance) + guidance.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/guidance/hybridpath_guidance/launch/hybridpath_guidance.launch.py b/guidance/hybridpath_guidance/launch/hybridpath_guidance.launch.py new file mode 100644 index 00000000..45afd514 --- /dev/null +++ b/guidance/hybridpath_guidance/launch/hybridpath_guidance.launch.py @@ -0,0 +1,16 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + hybridpath_guidance_node = Node( + package='hybridpath_guidance', + executable='hybridpath_guidance_node.py', + name='hybridpath_guidance_node', + parameters=[os.path.join(get_package_share_directory('hybridpath_guidance'),'config','hybridpath_guidance_config.yaml')], + output='screen', + ) + return LaunchDescription([ + hybridpath_guidance_node + ]) diff --git a/guidance/hybridpath_guidance/package.xml b/guidance/hybridpath_guidance/package.xml new file mode 100644 index 00000000..0d828000 --- /dev/null +++ b/guidance/hybridpath_guidance/package.xml @@ -0,0 +1,21 @@ + + + + hybridpath_guidance + 0.0.0 + This package provides the implementation of hybrid path guidance for the Vortex ASV. + vortex + MIT + + rclpy + nav_msgs + python-transforms3d-pip + geometry_msgs + vortex_msgs + + python3-pytest + + + ament_cmake + + diff --git a/guidance/hybridpath_guidance/tests/test_hybridpath.py b/guidance/hybridpath_guidance/tests/test_hybridpath.py new file mode 100644 index 00000000..7b5119b0 --- /dev/null +++ b/guidance/hybridpath_guidance/tests/test_hybridpath.py @@ -0,0 +1,31 @@ +from hybridpath_guidance.hybridpath import HybridPathGenerator, HybridPathSignals +from geometry_msgs.msg import Point +import numpy as np +import unittest + +class TestHybridPath(unittest.TestCase): + + def setUp(self): + WP = [Point(x=0.0, y=0.0), Point(x=10.0, y=0.0), Point(x=10.0, y=10.0), Point(x=0.0, y=10.0), Point(x=0.0, y=0.0)] + r = 1 + lambda_val = 0.15 + self.generator = HybridPathGenerator(WP, 1, lambda_val) + self.path = self.generator.path + + def test_create_A_matrix(self): + A = self.generator._construct_A_matrix() + A_expected = np.array([[1.0, 0.0, 0.0, 0.0], + [1.0, 1.0, 1.0, 1.0], + [0.0, 1.0, 0.0, 0.0], + [0.0, 1.0, 2.0, 3.0]]) + self.assertEqual(A.all(), A_expected.all()) + + def test_calculate_subpath_coeffs_j_is_zero(self): + coeffs = self.generator._calculate_subpath_coeffs(0) + coeffs_expected = (np.array([0.0, 10.0, 10.0, 1.5]), + np.array([0.0, 0.0, 0.0, 1.5])) + self.assertEqual(coeffs[0].all(), coeffs_expected[0].all()) + self.assertEqual(coeffs[1].all(), coeffs_expected[1].all()) + +if __name__ == '__main__': + unittest.main() \ No newline at end of file diff --git a/motion/hybridpath_controller/CMakeLists.txt b/motion/hybridpath_controller/CMakeLists.txt new file mode 100644 index 00000000..afec5754 --- /dev/null +++ b/motion/hybridpath_controller/CMakeLists.txt @@ -0,0 +1,35 @@ +cmake_minimum_required(VERSION 3.8) +project(hybridpath_controller) + +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(vortex_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) + +ament_python_install_package(${PROJECT_NAME}) + +install(DIRECTORY + launch + config + DESTINATION share/${PROJECT_NAME} +) + +install(PROGRAMS + hybridpath_controller/hybridpath_controller_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/motion/hybridpath_controller/README.md b/motion/hybridpath_controller/README.md new file mode 100755 index 00000000..cde9b56b --- /dev/null +++ b/motion/hybridpath_controller/README.md @@ -0,0 +1,202 @@ +# Hybrid Path Controller + +This package provides the implementation of hybrid path controller for the Vortex ASV. + +## Usage + +To use the hybrid path guidance launch it using: `ros2 launch hybridpath_controller hybridpath_controller.launch` +Or alternatively, run it together with the hybridpath guidance using the launch file `hybridpath.launch.py` in asv_setup + +## Configuration + +You can configure the behavior of the hybrid path controller by modifying the parameters in the `config` directory. + +## Theory + +### Vessel Model +For our controller we use the simplified 3 degree-of-freedom (DOF) model +$$\dot{\eta} = \textbf{R}(\psi) \nu$$ +$$\textbf{M}\dot{\nu} = -\textbf{D}\nu + \tau$$ + +where $\nu = [u,v,r]^T$ is the velocity of the vessel in the body-fixed frame {b} and $\eta = [p,\psi]^T = [x,y,\psi]^T$ is the position and heading of the vessel given in the North-East-Down frame {n}. $\textbf{M}$ is the inertia matrix, $\textbf{D}$ is the linear damping matrix and $\tau$ is the control input in {n}. $\textbf{R}(\psi)$ is the 3DOF rotation matrix with the property that $\dot{\textbf{R}}(\psi) = \textbf{R}\textbf{S}(r)$ where $\textbf{S}(r)$ is skew-symmetric. Thus: +```math +\textbf{R}(\psi) := \begin{bmatrix} \cos\psi & -\sin\psi & 0 \\ \sin\psi & \cos\psi & 0 \\ 0 & 0 & 1 +\end{bmatrix} +``` +```math +\textbf{S}(r) := \begin{bmatrix} 0 & -r & 0 \\ r & 0 & 0 \\ 0 & 0 & 0 +\end{bmatrix} +``` + +The control input $\tau$ contains the thrust forces acting on the vessel: +```math +\tau = \begin{bmatrix} \tau_X \\ \tau_Y \\ \tau_N \end{bmatrix} = \begin{bmatrix} F_X \\ F_Y \\ l_x F_Y - l_y F_X \end{bmatrix} +``` + +where $F_X$ and $F_Y$ are the forces in the respective indexed directions and $l_x$ and $l_y$ are the arms from which $\tau_N$ are acting on. + +### The maneuvering problem +The maneuvering problem for an ASV is comprimised of two tasks: +1. $\textbf{Geometric task:}$ Given a desired pose $\eta_d(s(t))$, force the vessels pose $\eta(t)$ to converge to the desired path: $$\lim_{{t \to \infty}} [\eta(t) - \eta_d(s(t))] = 0$$ + +2. $\textbf{Dynamic task:}$ Satisfy one or more of the following assignments: + 1. $\textbf{Speed assignment:}$ Force the path speed $\dot{s}(t)$ to converge to the desired speed $v_s(t, s(t))$: + $$\lim_{{t \to \infty}} [\dot{s}(t) - v_s(t, s(t))] = 0$$ + + 2. $\textbf{Time assignment:}$ Force the path variable $s$ to converge to a desired time signal $v_t(t)$: + $$\lim_{{t \to \infty}} [s(t) - v_t()t,s(t)] = 0$$ + +The maneuvering problem highlights the superior goal of convergence to the path and to fulfill the dynamic assignment. We will be considering only the geometric task and the speed assignment. + +For the speed assignment, the guidance system needs to generate the speed profile $`v_s(t,s(t))`$ and its derivatives. We let the desired path speed $`u_d(t)`$ (in m/s) be a commanded input speed. We have that: +```math +|\dot{p}_d(s(t))| = \sqrt{x^s_d(s(t))^2\dot{s}(t)^2 + y^s_d(s(t))^2\dot{s}(t)^2} \\ = \sqrt{x^s_d(s(t))^2 + y^s_d(s(t))^2} |v_s(t,s(t))| = |u_d(t)| +``` +which must hold along the path. The speed assignment is thus, by definition: +```math +v_s(t,s(t)) := \frac{u_d(t)}{\sqrt{x^s_d(s(t))^2 + y^s_d(s(t))^2}} +``` +Setting the commanded input $`u_d(t) = 0`$ m/s will stop the vessel on the path, while setting $`u_d(t) > 0`$ m/s will move the vessel in positive direction along the path and $`u_d(t) < 0`$ m/s will move it in negative direction. + +Combining the generated path from [the path generator](../../guidance/hybridpath_guidance/README.md) with a dynamic assignment along the path ensures that dynamic and temporal requirements are satisfied. The maneuvering problem states that the geometry of the path and the dynamical behaviour along the path can be defined and controlled separately. This means that a path can be generated, and the speed can be controlled without having to regenerate a new path. +### Nonlinear adaptive backstepping +To design the maneuvering controller, we use the nonlinear backstepping technique. In short, backstepping is a recursive technique breaking the design problem of the full system down to a sequence of sub-problems on lower-order systems, and by recursively use some states as virtual control inputs to obtain the intermediate control laws using the appropriate Control Lyapunov Functions (CLF). The design is done in two steps. After the first step, we define the dynamic update law for the parametric value $\dot{s}(t)$ to fulfill the control objective. + +#### Step 1 +We define the error state variables: +```math +z_1 := \textbf{R}(\psi)^T(\eta - \eta_d(s)), \\z_2 := \nu - \alpha_1, \\ \omega := \dot{s} - v_s(t,s), +``` +where $`\alpha_1`$ is the virtual control to be specified later. The total derivative of $`z_1`$ is: +```math +\dot{z}_1 = \dot{\textbf{R}}(\psi)^T(\eta - \eta_d(s)) + \textbf{R}(\psi)^T(\dot{\eta} - \eta^s_d(s)\dot{s}) +``` +```math + = -\textbf{S}(r)\textbf{R}(\psi)^T(\eta-\eta_d(s)) + \nu - \textbf{R}(\psi)^T\eta^s_d(s)\dot{s} +``` +```math + = -\textbf{S}(r)z_1 + z_2 + \alpha_1 - \textbf{R}(\psi)^T\eta^s_d(s)(\omega + v_s(t,s)) +``` + +The first CLF is defined as: +```math +V_1 := \frac{1}{2}z^T_1z_1 +``` +and its total derivative: +```math +\dot{V}_1 = \frac{1}{2}\dot{z}^T_1z_1 + \frac{1}{2}z^T_1\dot{z}_1 + ``` +```math += \frac{1}{2}(-\textbf{S}(r)z_1 + z_2 + \alpha_1 - \textbf{R}(\psi)^T\eta^s_d(s)(\omega + v_s(t,s)))^Tz_1 +``` +```math + + \frac{1}{2}z^T_1(-\textbf{S}(r)z_1 + z_2 + \alpha_1 - \textbf{R}(\psi)^T\eta^s_d(s)(\omega + v_s(t,s))) +``` +```math + = \frac{1}{2}(-z^T_1\textbf{S}(r)z_1 + z^T_1z_2 + z^T_1\alpha_1 +``` +```math + -z^T_1[\textbf{R}(\psi)^T\eta^s_d(s)(\omega + v_s(t,s))] - z^T_1\textbf{S}(r)z_1 + z^T_1z_2 + z^T_1\alpha_1 +``` +```math + -z^T_1[\textbf{R}(\psi)^T\eta^s_d(s)(\omega + v_s(t,s))]) +``` +```math + = z^T_1z_2 + z^T_1[\alpha_1 - \textbf{R}(\psi)^T\eta^s_d(s)(\omega + v_s(t,s))] +``` + +and futhermore, its derivative with respect to $s$ is: +```math +V^s_1 = \frac{1}{2} z^{s\:T}z_1 + \frac{1}{2}z^T_1z^s_1 +``` +```math += \frac{1}{2}(-\textbf{R}(\psi)^T\eta^s_d(s))^Tz_1 + \frac{1}{2}z^T_1(-\textbf{R}(\psi)^T\eta^s_d(s)) +``` +```math += -z^T_1 \textbf{R}(\psi)^T\eta^s_d(s) +``` +Now we use Young's inequality, such that: +```math +\dot{V}_1 = z^T_1z_2 + z^T_1[\alpha_1 - \textbf{R}(\psi)^T\eta^s_d(s)(\omega + v_s(t,s))] +``` +```math +\leq \frac{1}{4\kappa}z^T_2z_2 + z^T_1[\kappa z_1 + \alpha_1 - \textbf{R}(\psi)^T\eta^s_d(s)(\omega + v_s(t,s))] +``` +We choose our first virtual control $\alpha_1$ and tuning function $\rho_1$ as: +```math +\alpha_1 = -\textbf{K}_1z_1 + \textbf{R}(\psi)^T\eta^s_d(s)v_s(t,s) - \kappa z_1, \; \textbf{K}_1 = \textbf{K}^T_1 > 0, \; \kappa > 0 +``` +```math +\rho_1 = -z^T_1 \textbf{R}(\psi)^T\eta^s_d(s) = V^s_1 +``` +Plugging in $\alpha_1$ and $\rho_1$ into the inequality gives: +```math +\dot{V}_1 \leq -z^T_1 \textbf{K}_1 z_1 + \rho_1 \omega + \frac{1}{4\kappa}z^T_2 z_2 +``` +where we postpone dealing with the coupling term involving $z_2$ until the next step. + +#### Dynamic Update Law Acting in Output Space: +The dynamic update law is constructed to bridge the path following objective with the speed assignment. We examine two candidates so that the hybrid path signal being sent from the guidance system can be controlled. +- Tracking update law: Choosing: +```math +\omega = 0 \Rightarrow \dot{s} = v_s(t,s) +``` +satisfies the dynamic task. This also gives: +```math +\dot{V}_1 \leq -z^T_1 \textbf{K}_1 z_1. +``` +Hence the tracking update law renders $\dot{V}_1$ negative definite and the equilibrium $z_1 = 0$ uniformly globally exponentially stable (UGES). + +#### Step 2 +The second CLF is defined as: +```math +V_2 := V_1 + \frac{1}{2} z^T_2 \textbf{M} z_2 +``` +and its total derivative: +```math +\dot{V}_2 = \dot{V}_1 + z^T_2 \textbf{M} \dot{z}_2 +``` +```math += -z^T_1 \textbf{K} z_1 + \frac{1}{4\kappa}z^T_2 z_2 + z^T_2(\textbf{M}\dot{\nu}- \textbf{M}\dot{\alpha}_1) +``` +Plugging in our 3DOF model for $\textbf{M}\dot{\nu}$ we obtain: +```math +\dot{V}_2 = -z^T_1 \textbf{K}_1 z_1 + \frac{1}{4\kappa}z^T_2 z_2 ++ z^T_2(-\textbf{D}\nu + \tau - \textbf{M}\alpha_1) +``` +We now choose $\tau$ to stabilize our second CLF: +```math +\tau = -\textbf{K}_2 z_2 + \textbf{D}\nu + \textbf{M}\dot{\alpha}_1, \; \textbf{K}_2 = \textbf{K}^T_2 > 0 +``` +And plugging into the equation for $\dot{V}_2$: +```math +\dot{V}_2 \leq -z^T_1 \textbf{K}_1 z_1 - z^T_2 (\textbf{K}_2 - \frac{1}{4\kappa}z_2) \leq 0 +``` +Thus $\tau$ renders $\dot{V}_2$ negative definite and thereby the equilibrium point $(z_1, z_2) = (0,0)$ UGES. Finally we must find an expression for $\dot{\alpha}_1$ since it appears in the expression for $\tau$. +```math +\dot{\alpha}_1 = \textbf{K}_1 \dot{z}_1 + \dot{\textbf{R}}(\psi)^T\eta^s_d(s)v_s(t,s) + \textbf{R}(\psi)^T\dot{\eta}^s_d(s)v_s(t,s) + \textbf{R}(\psi)^T\eta^s_d(s)\dot{v}_s(t,s) +``` +```math += -\textbf{K}_1 \dot{z}_1 - \textbf{S}(r)\textbf{R}(psi)^T\eta^s_d(s)v_s(t,s) + \textbf{R}(\psi)^T\eta^{s^2}_d(s)\dot{s}v_s(t,s) + \textbf{R}(\psi)^T \eta^s_d(s)(v^t_s(t, s) + v^s_s(t, s)\dot{s}) +``` +By plugging in for $\dot{z}_1$ the expression can be further simplified: +```math +\dot{\alpha}_1 = \textbf{K}_1 \textbf{S}(r)z_1 - \textbf{K}_1\nu - \textbf{S}(r)\textbf{R}(\psi)^T\eta^s_d(s)v_s(t,s) +``` +```math ++ \textbf{R}(\psi)^T\eta^s_d(s) v^t_s(t,s) + [\textbf{K}_1 \textbf{R}(\psi)^T\eta^s_d(s) + \textbf{R}(\psi)^T\eta^{s^2}(s)v_s(t,s) +``` +```math ++ \textbf{R}(\psi)^T\eta^s_d(s)v^s_s(t,s)]\dot{s} +``` +The terms inside the square brackets constitutes $\alpha^s_1$. If we now define: +```math +\sigma_1 := \textbf{K}_1 \textbf{S}(r)z_1 - \textbf{K}_1\nu - \textbf{S}(r)\textbf{R}(\psi)^T\eta^s_d(s)v_s(t,s) + \textbf{R}(\psi)^T\eta^s_d(s)v^t_s(t,s) +``` +We can write: +```math +\dot{\alpha}_1 = \sigma_1 + \alpha^s_1\dot{s} +``` + +### Source +https://folk.ntnu.no/rskjetne/Publications/SkjetnePhDthesis_B5_compressed.pdf \ No newline at end of file diff --git a/motion/hybridpath_controller/config/hybridpath_controller_config.yaml b/motion/hybridpath_controller/config/hybridpath_controller_config.yaml new file mode 100644 index 00000000..59c040f2 --- /dev/null +++ b/motion/hybridpath_controller/config/hybridpath_controller_config.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + hybridpath_controller: + K1_diag: [0.1, 0.1, 0.1] # First gain matrix + K2_diag: [0.1, 0.1, 0.1] # Second gain matrix \ No newline at end of file diff --git a/motion/hybridpath_controller/hybridpath_controller/__init__.py b/motion/hybridpath_controller/hybridpath_controller/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/motion/hybridpath_controller/hybridpath_controller/adaptive_backstep.py b/motion/hybridpath_controller/hybridpath_controller/adaptive_backstep.py new file mode 100644 index 00000000..daa373bf --- /dev/null +++ b/motion/hybridpath_controller/hybridpath_controller/adaptive_backstep.py @@ -0,0 +1,122 @@ +import numpy as np +from nav_msgs.msg import Odometry +from vortex_msgs.msg import HybridpathReference +from transforms3d.euler import quat2euler + +class AdaptiveBackstep: + def __init__(self, K1: np.ndarray, K2: np.ndarray, M: np.ndarray, D: np.ndarray) -> None: + self.K_1 = K1 + self.K_2 = K2 + self.M = M + self.D = D + + def control_law(self, state: Odometry, reference: HybridpathReference) -> np.ndarray: + """ + Calculates the control input based on the state and reference. + + Args: + state (Odometry): The current state of the system. + reference (HybridpathReference): The reference to follow. + + Returns: + np.ndarray: The control input. + """ + + # Transform the Odometry message to a state vector + state = self.odom_to_state(state) + + # Extract values from the state and reference + eta = state[:3] + nu = state[3:] + w = reference.w + v_s = reference.v_s + v_ss = reference.v_ss + eta_d = np.array([reference.eta_d.x, reference.eta_d.y, reference.eta_d.theta]) + eta_d_s = np.array([reference.eta_d_s.x, reference.eta_d_s.y, reference.eta_d_s.theta]) + eta_d_ss = np.array([reference.eta_d_ss.x, reference.eta_d_ss.y, reference.eta_d_ss.theta]) + + # Get R_transposed and S + R_trps = self.rotationmatrix_in_yaw_transpose(eta[2]) + S = self.skew_symmetric_matrix(nu[2]) + + # Define error signals + eta_error = eta - eta_d + eta_error[2] = self.ssa(eta_error[2]) + + z1 = R_trps @ eta_error + alpha1 = -self.K_1 @ z1 + R_trps @ eta_d_s * v_s + + z2 = nu - alpha1 + + sigma1 = self.K_1 @ (S @ z1) - self.K_1 @ nu - S @ (R_trps @ eta_d_s) * v_s + + ds_alpha1 = self.K_1 @ (R_trps @ eta_d_s) + R_trps @ eta_d_ss * v_s + R_trps @ eta_d_s * v_ss + + # Control law ## Må endres om de ulineære matrisene skal brukes + tau = -self.K_2 @ z2 + self.calculate_coriolis_matrix(nu) + self.D @ nu + self.M @ sigma1 + self.M @ ds_alpha1 * (v_s + w) + + # Add constraints to tau # This should be improved + # for i in range(len(tau)): + # if tau[i] > self.tau_max[i]: + # tau[i] = self.tau_max[i] + # elif tau[i] < -self.tau_max[i]: + # tau[i] = -self.tau_max[i] + + return tau + + @staticmethod + def calculate_coriolis_matrix(nu): + C_A = np.array([ + [0, 0, -82.5], + [0, 0, 5.5], + [82.5, -5.5, 0] + ]) + return C_A @ nu + + @staticmethod + def rotationmatrix_in_yaw_transpose(psi: float) -> np.ndarray: + R = np.array([[np.cos(psi), -np.sin(psi), 0], + [np.sin(psi), np.cos(psi), 0], + [0, 0, 1]]) + R_trps = np.transpose(R) + return R_trps + + @staticmethod + def skew_symmetric_matrix(r: float) -> np.ndarray: + S = np.array([[0, -r, 0], + [r, 0, 0], + [0, 0, 0]]) + return S + + @staticmethod + def ssa(angle: float) -> float: + wrpd_angle = (angle + np.pi) % (2.0*np.pi) - np.pi + return wrpd_angle + + @staticmethod + def odom_to_state(msg: Odometry) -> np.ndarray: + """ + Converts an Odometry message to a state 3DOF vector. + + Args: + msg (Odometry): The Odometry message to convert. + + Returns: + np.ndarray: The state vector. + """ + x = msg.pose.pose.position.x + y = msg.pose.pose.position.y + orientation_q = msg.pose.pose.orientation + orientation_list = [ + orientation_q.w, orientation_q.x, orientation_q.y, orientation_q.z + ] + + # Convert quaternion to Euler angles + (roll, pitch, yaw) = quat2euler(orientation_list) + + u = msg.twist.twist.linear.x + v = msg.twist.twist.linear.y + r = msg.twist.twist.angular.z + + state = np.array([x, y, yaw, u, v, r]) + return state diff --git a/motion/hybridpath_controller/hybridpath_controller/hybridpath_controller_node.py b/motion/hybridpath_controller/hybridpath_controller/hybridpath_controller_node.py new file mode 100755 index 00000000..d74f1abb --- /dev/null +++ b/motion/hybridpath_controller/hybridpath_controller/hybridpath_controller_node.py @@ -0,0 +1,78 @@ +#!/usr/bin/env python3 + +import rclpy +import numpy as np +from rclpy.node import Node +from hybridpath_controller.adaptive_backstep import AdaptiveBackstep +from geometry_msgs.msg import Wrench +from nav_msgs.msg import Odometry +from vortex_msgs.msg import HybridpathReference + +class HybridPathControllerNode(Node): + def __init__(self): + super().__init__("hybridpath_controller_node") + self.declare_parameters( + namespace='', + parameters=[ + ('hybridpath_controller.K1_diag', [10.0, 10.0, 10.0]), + ('hybridpath_controller.K2_diag', [60.0, 60.0, 60.0]), + ('physical.inertia_matrix', [50.0, 50.0, 50.0]), + ('physical.damping_matrix_diag', [10.0, 10.0, 5.0]) + ]) + + self.state_subscriber_ = self.state_subscriber_ = self.create_subscription(Odometry, '/sensor/seapath/odom/ned', self.state_callback, 1) + self.hpref_subscriber_ = self.create_subscription(HybridpathReference, 'guidance/hybridpath/reference', self.reference_callback, 1) + self.wrench_publisher_ = self.create_publisher(Wrench, 'thrust/wrench_input', 1) + + # Get parameters + K1_diag = self.get_parameter('hybridpath_controller.K1_diag').get_parameter_value().double_array_value + K2_diag = self.get_parameter('hybridpath_controller.K2_diag').get_parameter_value().double_array_value + D_diag = self.get_parameter('physical.damping_matrix_diag').get_parameter_value().double_array_value + M = self.get_parameter('physical.inertia_matrix').get_parameter_value().double_array_value + + # Transform parameters to diagonal matrices + K1 = np.diag(K1_diag) + K2 = np.diag(K2_diag) + D = np.diag(D_diag) + + # Reshape M to a 3x3 matrix + M = np.reshape(M, (3, 3)) + + # Create controller object + self.AB_controller_ = AdaptiveBackstep(K1, K2, M, D) + + controller_period = 0.1 + self.controller_timer_ = self.create_timer(controller_period, self.controller_callback) + + self.get_logger().info("hybridpath_controller_node started") + + def state_callback(self, msg: Odometry): + """ + Callback function for the Odometry message. This function saves the state message. + """ + self.state_odom = msg + + def reference_callback(self, msg: HybridpathReference): + self.reference = msg + + def controller_callback(self): + """ + Callback function for the controller timer. This function calculates the control input and publishes the control input. + """ + if hasattr(self, 'state_odom') and hasattr(self, 'reference'): + control_input = self.AB_controller_.control_law(self.state_odom, self.reference) + wrench_msg = Wrench() + wrench_msg.force.x = control_input[0] + wrench_msg.force.y = control_input[1] + wrench_msg.torque.z = control_input[2] + self.wrench_publisher_.publish(wrench_msg) + +def main(args=None): + rclpy.init(args=args) + node = HybridPathControllerNode() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + +if __name__ == "__main__": + main() diff --git a/motion/hybridpath_controller/launch/hybridpath_controller.launch.py b/motion/hybridpath_controller/launch/hybridpath_controller.launch.py new file mode 100755 index 00000000..683fda55 --- /dev/null +++ b/motion/hybridpath_controller/launch/hybridpath_controller.launch.py @@ -0,0 +1,19 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + hybridpath_controller_node = Node( + package='hybridpath_controller', + executable='hybridpath_controller_node.py', + name='hybridpath_controller_node', + parameters=[ + os.path.join(get_package_share_directory('hybridpath_controller'), 'config', 'hybridpath_controller_config.yaml'), + os.path.join(get_package_share_directory('asv_setup'), 'config', 'robots', 'freya.yaml') + ], + output='screen', + ) + return LaunchDescription([ + hybridpath_controller_node + ]) diff --git a/motion/hybridpath_controller/package.xml b/motion/hybridpath_controller/package.xml new file mode 100644 index 00000000..2757b981 --- /dev/null +++ b/motion/hybridpath_controller/package.xml @@ -0,0 +1,23 @@ + + + + hybridpath_controller + 0.0.0 + This package provides the implementation of hybrid path controller for the Vortex ASV. + vortex + MIT + + ament_cmake_python + + rclpy + python-transforms3d-pip + geometry_msgs + vortex_msgs + + python3-pytest + + + + ament_cmake + + diff --git a/motion/hybridpath_controller/tests/test_hybridpath_controller.py b/motion/hybridpath_controller/tests/test_hybridpath_controller.py new file mode 100644 index 00000000..52919a77 --- /dev/null +++ b/motion/hybridpath_controller/tests/test_hybridpath_controller.py @@ -0,0 +1,82 @@ +from hybridpath_controller.hybridpath_controller_node import HybridPathControllerNode + +import launch +import launch_ros +import launch_ros.actions +import launch_testing.actions + +import unittest +import numpy as np +import pytest +import rclpy + +@pytest.mark.rostest +def generate_test_description(): + hybridpath_controller_node = launch_ros.actions.Node( + package='hybridpath_controller', + executable='hybridpath_controller_node', + name='hybridpath_controller_node', + output='screen' + ) + + return ( + launch.LaunchDescription([ + hybridpath_controller_node, + + launch_testing.actions.ReadyToTest() + ]), + { + 'controller': hybridpath_controller_node + } + ) + +class TestHybridPathControllerNode(unittest.TestCase): + @classmethod + def setUpClass(cls): + rclpy.init() + + @classmethod + def tearDownClass(cls): + rclpy.shutdown() + + def setUp(self): + self.node = HybridPathControllerNode() + + def tearDown(self): + self.node.destroy_node() + + def test_control_law(self): + """ + WARNING! Expects following values used in AB_controller_: + I = np.eye(3) + kappa = 0.5 + + K_1 = np.diag([10, 10, 10]) + self.K_1_tilde = K_1 + kappa*I + self.K_2 = np.diag([60, 60, 30]) + self.tau_max = np.array([41.0, 50.0, 55.0]) # Må tilpasses thrusterne + + m = 50 + self.M = np.diag([m, m, m]) + self.D = np.diag([10, 10, 5]) + """ + w_ref = 0.03819000726434428 + v_ref = 0.1001407579811512 + v_ref_t = 0.0 + v_ref_s = 0.0108627157420586 + + eta = np.array([1.0, 1.0, 0.0]) + nu = np.array([0.0, 0.0, 0.0]) + eta_d = np.array([10.00035914, 0.24996664, 1.56654648]) + eta_d_s = np.array([0.04243858, 9.98585381, -0.33009297]) + eta_d_ss = np.array([3.29165665, -1.09721888, -11.90686869]) + + expected_tau = np.array([41.0, 2.25865772, 3.32448561]) + + tau = self.node.AB_controller_.control_law(eta, nu, w_ref, v_ref, v_ref_t, v_ref_s, eta_d, eta_d_s, eta_d_ss) + + print(tau) + print(expected_tau) + assert np.isclose(tau[0], expected_tau[0], atol=1e-8) + assert np.isclose(tau[1], expected_tau[1], atol=1e-8) + assert np.isclose(tau[2], expected_tau[2], atol=1e-8) \ No newline at end of file diff --git a/motion/lqr_controller/CMakeLists.txt b/motion/lqr_controller/CMakeLists.txt new file mode 100644 index 00000000..cafa3ad4 --- /dev/null +++ b/motion/lqr_controller/CMakeLists.txt @@ -0,0 +1,27 @@ +cmake_minimum_required(VERSION 3.8) +project(lqr_controller) + +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(vortex_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) + +ament_python_install_package(${PROJECT_NAME}) + +install(DIRECTORY + launch + config + DESTINATION share/${PROJECT_NAME} +) + +install(PROGRAMS +lqr_controller/lqr_controller_node.py + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() \ No newline at end of file diff --git a/motion/lqr_controller/config/lqr_config.yaml b/motion/lqr_controller/config/lqr_config.yaml new file mode 100644 index 00000000..ac497221 --- /dev/null +++ b/motion/lqr_controller/config/lqr_config.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + lqr_controller: + Q: [20.0, 20.0, 1.0, 1.0, 1.0, 1.0] # State costs for [x, y, heading, surge, sway, yaw] + R: [1.0, 1.0, 1.0] # Control cost weight \ No newline at end of file diff --git a/motion/lqr_controller/launch/lqr_controller.launch.py b/motion/lqr_controller/launch/lqr_controller.launch.py new file mode 100644 index 00000000..03ceca10 --- /dev/null +++ b/motion/lqr_controller/launch/lqr_controller.launch.py @@ -0,0 +1,19 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + lqr_controller_node = Node( + package='lqr_controller', + executable='lqr_controller_node.py', + name='lqr_controller_node', + parameters=[ + os.path.join(get_package_share_directory('lqr_controller'),'config','lqr_config.yaml'), + os.path.join(get_package_share_directory('asv_setup'), 'config', 'robots', 'freya.yaml') + ], + output='screen', + ) + return LaunchDescription([ + lqr_controller_node + ]) diff --git a/motion/lqr_controller/lqr_controller/__init__.py b/motion/lqr_controller/lqr_controller/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/motion/lqr_controller/lqr_controller/conversions.py b/motion/lqr_controller/lqr_controller/conversions.py new file mode 100644 index 00000000..92f14cfb --- /dev/null +++ b/motion/lqr_controller/lqr_controller/conversions.py @@ -0,0 +1,59 @@ +import numpy as np + +from transforms3d.euler import euler2quat, quat2euler +from nav_msgs.msg import Odometry +from geometry_msgs.msg import PoseStamped + + +def odometrymsg_to_state(msg): + x = msg.pose.pose.position.x + y = msg.pose.pose.position.y + orientation_q = msg.pose.pose.orientation + orientation_list = [ + orientation_q.w, orientation_q.x, orientation_q.y, orientation_q.z + ] + + # Convert quaternion to Euler angles, extract yaw + yaw = quat2euler(orientation_list)[2] + + vx = msg.twist.twist.linear.x + vy = msg.twist.twist.linear.y + vyaw = msg.twist.twist.angular.z + + state = np.array([x, y, yaw, vx, vy, vyaw]) + return state + +def state_to_odometrymsg(state): + orientation_list_next = euler2quat(0, 0, state[2]) + + odometry_msg = Odometry() + odometry_msg.pose.pose.position.x = state[0] + odometry_msg.pose.pose.position.y = state[1] + odometry_msg.pose.pose.position.z = 0.0 + odometry_msg.pose.pose.orientation.w = orientation_list_next[0] + odometry_msg.pose.pose.orientation.x = orientation_list_next[1] + odometry_msg.pose.pose.orientation.y = orientation_list_next[2] + odometry_msg.pose.pose.orientation.z = orientation_list_next[3] + + return odometry_msg + +def state_to_posestamped(state, frame_id, stamp): + orientation_list_next = euler2quat(0, 0, state[2]) + + posestamped_msg = PoseStamped() + + posestamped_msg.header.frame_id = frame_id + posestamped_msg.header.stamp = stamp + + posestamped_msg.pose.position.x = state[0] + posestamped_msg.pose.position.y = state[1] + posestamped_msg.pose.position.z = 0.0 + posestamped_msg.pose.orientation.w = orientation_list_next[0] + posestamped_msg.pose.orientation.x = orientation_list_next[1] + posestamped_msg.pose.orientation.y = orientation_list_next[2] + posestamped_msg.pose.orientation.z = orientation_list_next[3] + + return posestamped_msg + +def ssa(angle): + return (angle + np.pi) % (2 * np.pi) - np.pi \ No newline at end of file diff --git a/motion/lqr_controller/lqr_controller/lqr_controller.py b/motion/lqr_controller/lqr_controller/lqr_controller.py new file mode 100644 index 00000000..7c7b6f67 --- /dev/null +++ b/motion/lqr_controller/lqr_controller/lqr_controller.py @@ -0,0 +1,39 @@ +import numpy as np +from scipy.linalg import solve_continuous_are + +class LQRController: + def __init__(self, M: float, D: list[float], Q: list[float], R: list[float], heading_ref: float = 0.0): + self.M = M + self.M_inv = np.linalg.inv(self.M) + self.D = D + + self.A = np.zeros((6,6)) + self.B = np.zeros((6,3)) + self.C = np.zeros((3,6)) + self.C[:3, :3] = np.eye(3) + + self.Q = np.diag(Q) + print(f"Q: {self.Q}") + self.R = np.diag(R) + + self.linearize_model(heading_ref) + + def calculate_control_input(self, x, x_ref): + tau = -self.K_LQR @ x + self.K_r @ x_ref + return tau + + def linearize_model(self, heading: float) -> tuple[np.ndarray, np.ndarray]: + R = np.array( + [[np.cos(heading), -np.sin(heading), 0], + [np.sin(heading), np.cos(heading), 0], + [0, 0, 1]] + ) + + self.A[:3,3:] = R + self.A[3:, 3:] = - self.M_inv @ self.D + + self.B[3:,:] = self.M_inv + + self.P = solve_continuous_are(self.A, self.B, self.Q, self.R) + self.K_LQR = np.dot(np.dot(np.linalg.inv(self.R), self.B.T), self.P) + self.K_r = np.linalg.inv(self.C@np.linalg.inv(self.B @ self.K_LQR - self.A) @ self.B) \ No newline at end of file diff --git a/motion/lqr_controller/lqr_controller/lqr_controller_node.py b/motion/lqr_controller/lqr_controller/lqr_controller_node.py new file mode 100644 index 00000000..18bebe0c --- /dev/null +++ b/motion/lqr_controller/lqr_controller/lqr_controller_node.py @@ -0,0 +1,80 @@ +#!/usr/bin/env python3 + +import rclpy +import numpy as np +from rclpy.node import Node +from geometry_msgs.msg import Wrench +from nav_msgs.msg import Odometry +from lqr_controller.lqr_controller import LQRController +from lqr_controller.conversions import odometrymsg_to_state +from time import sleep + +from rclpy.qos import QoSProfile, qos_profile_sensor_data, QoSReliabilityPolicy + +qos_profile = QoSProfile(depth=1, history=qos_profile_sensor_data.history, + reliability=QoSReliabilityPolicy.BEST_EFFORT) + +class LQRControllerNode(Node): + def __init__(self): + super().__init__("lqr_controller_node") + + self.declare_parameters( + namespace='', + parameters=[ + ('lqr_controller.Q', [10.0, 10.0, 5.0, 0.1, 1.0, 5.0]), + ('lqr_controller.R', [1.0, 1.0, 1.0]), + ('physical.inertia_matrix', [90.5, 0.0, 0.0, 0.0, 167.5, 12.25, 0.0, 12.25, 42.65]), + ('physical.damping_matrix_diag', [77.55, 162.5, 42.65]) + ]) + + self.state_subscriber_ = self.create_subscription(Odometry, "/sensor/seapath/odom/ned", self.state_cb, qos_profile=qos_profile) + self.guidance_subscriber_ = self.create_subscription(Odometry, "guidance/dp/reference", self.guidance_cb, 1) + self.wrench_publisher_ = self.create_publisher(Wrench, "thrust/wrench_input", 1) + + Q = self.get_parameter('lqr_controller.Q').get_parameter_value().double_array_value + R = self.get_parameter('lqr_controller.R').get_parameter_value().double_array_value + D_diag = self.get_parameter('physical.damping_matrix_diag').get_parameter_value().double_array_value + M = self.get_parameter('physical.inertia_matrix').get_parameter_value().double_array_value + + D = np.diag(D_diag) + M = np.reshape(M, (3, 3)) + + heading_ref = 0.0 + + self.LQR = LQRController(M, D, Q, R, heading_ref) + + self.x_ref = [0, 0, 0] + self.state = [0, 0, 0, 0, 0, 0] + + self.enabled = False + + controller_period = 0.1 + self.controller_timer_ = self.create_timer(controller_period, self.controller_callback) + + self.get_logger().info("lqr_controller_node started") + + def state_cb(self, msg): + self.state = odometrymsg_to_state(msg) + + def guidance_cb(self, msg): + self.x_ref = odometrymsg_to_state(msg)[:3] + + def controller_callback(self): + if hasattr(self, 'state') and hasattr(self, 'x_ref'): + self.LQR.linearize_model(self.state[2]) + control_input = self.LQR.calculate_control_input(self.state, self.x_ref) + wrench_msg = Wrench() + wrench_msg.force.x = control_input[0] + wrench_msg.force.y = control_input[1] + wrench_msg.torque.z = control_input[2] + self.wrench_publisher_.publish(wrench_msg) + +def main(args=None): + rclpy.init(args=args) + node = LQRControllerNode() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + +if __name__ == "__main__": + main() diff --git a/motion/lqr_controller/package.xml b/motion/lqr_controller/package.xml new file mode 100644 index 00000000..55cd3a82 --- /dev/null +++ b/motion/lqr_controller/package.xml @@ -0,0 +1,27 @@ + + + + lqr_controller + 0.0.0 + This is an implementation of the LQR DP controller for the ASV + vortex + MIT + + ament_cmake_python + + rclpy + python-transforms3d-pip + nav_msgs + geometry_msgs + vortex_msgs + std_msgs + numpy + matplotlib + scipy + + python3-pytest + + + ament_cmake + + diff --git a/simulation/asv_simulator/CMakeLists.txt b/simulation/asv_simulator/CMakeLists.txt new file mode 100644 index 00000000..6cb6a125 --- /dev/null +++ b/simulation/asv_simulator/CMakeLists.txt @@ -0,0 +1,27 @@ +cmake_minimum_required(VERSION 3.8) +project(asv_simulator) + +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(vortex_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) + +ament_python_install_package(${PROJECT_NAME}) + +install(DIRECTORY + launch + config + DESTINATION share/${PROJECT_NAME} +) + +install(PROGRAMS +asv_simulator/asv_simulator_node.py + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() \ No newline at end of file diff --git a/simulation/asv_simulator/README.md b/simulation/asv_simulator/README.md new file mode 100755 index 00000000..846f3a9f --- /dev/null +++ b/simulation/asv_simulator/README.md @@ -0,0 +1,16 @@ +# ASV simulator +This package provides the implementation of the asv_simulator for the Vortex ASV. + +## Usage + +Launch controller and guidance `ros2 launch asv_setup hybridpath.launch.py` +Launch the asv_simulator `ros2 launch asv_simulator asv_simulator.launch.py` + +## Configuration + +You can configure the settings of the simulator and waypoints to be used by modifying the parameters in the `config` directory. + +## Foxglove + +To visualize the simulation in Foxglove, launch foxglove bridge `ros2 run foxglove_bridge foxglove_bridge` + diff --git a/simulation/asv_simulator/asv_simulator/TF2Broadcaster.py b/simulation/asv_simulator/asv_simulator/TF2Broadcaster.py new file mode 100644 index 00000000..fe45d10e --- /dev/null +++ b/simulation/asv_simulator/asv_simulator/TF2Broadcaster.py @@ -0,0 +1,79 @@ +from geometry_msgs.msg import TransformStamped +from rclpy.node import Node +from tf2_ros import TransformBroadcaster +import math +import numpy as np + +def quaternion_from_euler(ai, aj, ak): + ai /= 2.0 + aj /= 2.0 + ak /= 2.0 + ci = math.cos(ai) + si = math.sin(ai) + cj = math.cos(aj) + sj = math.sin(aj) + ck = math.cos(ak) + sk = math.sin(ak) + cc = ci*ck + cs = ci*sk + sc = si*ck + ss = si*sk + + q = np.empty((4, )) + q[0] = cj*sc - sj*cs + q[1] = cj*ss + sj*cc + q[2] = cj*cs - sj*sc + q[3] = cj*cc + sj*ss + + return q + + +class TF2Broadcaster(Node): + def __init__(self, frame_id): + super().__init__('tf2_broadcaster') + self.tf_broadcaster = TransformBroadcaster(self) + + self.own_vessel_frame_id = frame_id + + def fixed_frame_broadcaster(self): + t = TransformStamped() + + t.header.stamp = self.get_clock().now().to_msg() + t.header.frame_id = 'map' + t.child_frame_id = 'world' + t.transform.translation.x = 0.0 + t.transform.translation.y = 0.0 + t.transform.translation.z = 0.0 + t.transform.rotation.x = 0.0 + t.transform.rotation.y = 0.0 + t.transform.rotation.z = 0.0 + t.transform.rotation.w = 0.0 + + self.tf_broadcaster.sendTransform(t) + + def handle_pose(self, state): + t = TransformStamped() + + # Read message content and assign it to + # corresponding tf variables + t.header.stamp = self.get_clock().now().to_msg() + t.header.frame_id = 'world' + t.child_frame_id = self.own_vessel_frame_id + + # Turtle only exists in 2D, thus we get x and y translation + # coordinates from the message and set the z coordinate to 0 + t.transform.translation.x = state[0] + t.transform.translation.y = state[1] + t.transform.translation.z = 0.0 + + # For the same reason, turtle can only rotate around one axis + # and this why we set rotation in x and y to 0 and obtain + # rotation in z axis from the message + q = quaternion_from_euler(0, 0, state[2]) + t.transform.rotation.x = q[0] + t.transform.rotation.y = q[1] + t.transform.rotation.z = q[2] + t.transform.rotation.w = q[3] + + # Send the transformation + self.tf_broadcaster.sendTransform(t) \ No newline at end of file diff --git a/simulation/asv_simulator/asv_simulator/__init__.py b/simulation/asv_simulator/asv_simulator/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/simulation/asv_simulator/asv_simulator/asv_simulator_node.py b/simulation/asv_simulator/asv_simulator/asv_simulator_node.py new file mode 100644 index 00000000..cef7a5d9 --- /dev/null +++ b/simulation/asv_simulator/asv_simulator/asv_simulator_node.py @@ -0,0 +1,249 @@ +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node + +from std_msgs.msg import Float32 +from geometry_msgs.msg import Wrench, PoseStamped, Point +from nav_msgs.msg import Odometry, Path +from vortex_msgs.msg import HybridpathReference +from vortex_msgs.srv import Waypoint + +import matplotlib.pyplot as plt +import numpy as np + +from asv_simulator.conversions import * +from asv_simulator.simulation import * +from asv_simulator.TF2Broadcaster import TF2Broadcaster + +class ASVSimulatorNode(Node): + def __init__(self): + super().__init__('asv_simulator_node') + + self.declare_parameters( + namespace='', + parameters=[ + ('asv_sim.plot_matplotlib_enabled', True), + ('asv_sim.progress_bar_enabled', True), + ('asv_sim.hybridpath_path_following', True), + ('asv_sim.T', 200), + ('asv_sim.waypoints', [0.0, 0.0]), + ('physical.inertia_matrix', [50.0, 50.0, 50.0]), + ('physical.damping_matrix_diag', [10.0, 10.0, 5.0]), + ]) + + self.plot_matplotlib_enabled = self.get_parameter('asv_sim.plot_matplotlib_enabled').get_parameter_value().bool_value + self.progress_bar_enabled = self.get_parameter('asv_sim.progress_bar_enabled').get_parameter_value().bool_value + self.hybridpath_path_following = self.get_parameter('asv_sim.hybridpath_path_following').get_parameter_value().bool_value + self.T = self.get_parameter('asv_sim.T').get_parameter_value().integer_value + waypoints_param = self.get_parameter('asv_sim.waypoints').get_parameter_value().double_array_value + self.waypoints = [Point(x=waypoints_param[i], y=waypoints_param[i+1], z=0.0) for i in range(0, len(waypoints_param), 2)] + D_diag = self.get_parameter('physical.damping_matrix_diag').get_parameter_value().double_array_value + M = self.get_parameter('physical.inertia_matrix').get_parameter_value().double_array_value + + # Init simulation parameters + self.D = np.diag(D_diag) + self.M = np.reshape(M, (3, 3)) + self.M_inv = np.linalg.inv(self.M) + + # Init TF2 broadcaster for frames and tfs + self.own_vessel_frame_id = "asv_pose" + self.tf_broadcaster = TF2Broadcaster(self.own_vessel_frame_id) + + # subscribe to thrust input and guidance + self.wrench_subscriber_ = self.create_subscription(Wrench, "thrust/wrench_input", self.wrench_input_cb, 1) + self.guidance_hp_subscriber_ = self.create_subscription(HybridpathReference, 'guidance/hybridpath/reference', self.guidance_hp_cb, 1) + self.guidance_dp_subscriber_ = self.create_subscription(Odometry, 'guidance/dp/reference', self.guidance_dp_cb, 1) + + # publish state (seapath) + self.state_publisher_ = self.create_publisher(Odometry, "/sensor/seapath/odom/ned", 1) + self.posestamped_publisher_ = self.create_publisher(PoseStamped, "/sensor/seapath/posestamped/ned", 1) + self.state_path_publisher_ = self.create_publisher(Path, "/sensor/seapath/state_path/ned", 1) + self.xref_path_publisher_ = self.create_publisher(Path, "/sensor/seapath/xref_path/ned", 1) + + self.yaw_ref_publisher_ = self.create_publisher(Float32, "/sensor/seapath/yaw_ref/ned", 1) + self.yaw_publisher_ = self.create_publisher(Float32, "/sensor/seapath/yaw/ned", 1) + + # send waypoints to guidance if hybridpath_path_following is enabled + if self.hybridpath_path_following: + self.waypoint_client = self.create_client(Waypoint, 'waypoint_list') + while not self.waypoint_client.wait_for_service(timeout_sec=1.0): + self.get_logger().info('service not available, waiting again...') + self.send_waypoint_request() + + # create timer + timer_period = 0.1 # seconds + self.timer = self.create_timer(timer_period, self.timer_callback) + + # Init state and control variables + self.state = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) + self.x_ref = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) + self.tau = np.array([0.0, 0.0, 0.0]) + + # Init historical pose (path) + self.state_path = Path() + self.xref_path = Path() + self.tau_history = np.zeros((0, 3)) + + realtime_factor = 10.0 + self.dt = timer_period*realtime_factor + self.num_steps_simulated = 0 + + self.N = 5000 # max Path length + self.max_steps = int(self.T/self.dt) + self.sim_finished = False + + # Publish initial state + self.state_publisher_.publish(state_to_odometrymsg(self.state)) + + self.get_logger().info("ASV simulator node initialized with parameters: \n" + + "Simulation time T = " + str(self.T) + "\n" + + "Matplotlib plotting enabled: " + str(self.plot_matplotlib_enabled) + "\n" + + "Hybridpath path following enabled: " + str(self.hybridpath_path_following) + "\n") + + + def send_waypoint_request(self): + req = Waypoint.Request() + req.waypoint = self.waypoints + future = self.waypoint_client.call_async(req) + future.add_done_callback(self.waypoint_response_callback) + + def waypoint_response_callback(self, future): + try: + response = future.result() + self.get_logger().info(f'Received response: {response}') + except Exception as e: + self.get_logger().error(f'Service call failed: {e}') + + def timer_callback(self): + self.simulate_step() + + if self.progress_bar_enabled and not self.sim_finished: + self.show_progressbar() + + if self.num_steps_simulated > self.max_steps and self.plot_matplotlib_enabled and not self.sim_finished: + self.plot_matplotlib() + self.sim_finished = True + + def show_progressbar(self): + progress = (self.num_steps_simulated / self.max_steps) * 100 + filled_length = int(progress // 10) + bar = '[' + '=' * filled_length + ' ' * (10 - filled_length) + ']' + self.get_logger().info(f"Progress: {bar} {progress:.1f}%") + + def guidance_hp_cb(self, msg): + if isinstance(msg, HybridpathReference): + self.x_ref[:3] = np.array([msg.eta_d.x, msg.eta_d.y, msg.eta_d.theta]) + self.yaw_ref_publisher_.publish(Float32(data=self.x_ref[2])) + else: + self.get_logger().error(f"Received message of type {type(msg).__name__}, expected HybridpathReference") + + def guidance_dp_cb(self, msg): + if isinstance(msg, Odometry): + self.x_ref[:3] = odometrymsg_to_state(msg)[:3] + self.yaw_ref_publisher_.publish(Float32(data=self.x_ref[2])) + else: + self.get_logger().error(f"Received message of type {type(msg).__name__}, expected Odometry") + + def wrench_input_cb(self, msg): + self.tau = np.array([msg.force.x, msg.force.y, msg.torque.z]) + tau_values = np.array([[msg.force.x, msg.force.y, msg.torque.z]]) + self.tau_history = np.append(self.tau_history, tau_values, axis=0) + + def update_path_element(self, path, new_pose, N): + if len(path.poses) < N: + path.poses.append(new_pose) + else: + # Shift elements and append new_pose + path.poses.pop(0) + path.poses.append(new_pose) + + def simulate_step(self): + x_next = RK4_integration_step(self.M_inv, self.D, self.state, self.tau, self.dt) + self.state = x_next + + # Pub odom + odometry_msg = state_to_odometrymsg(x_next) + self.state_publisher_.publish(odometry_msg) + + # Pub Posestamped + posestamped_msg = state_to_posestamped(x_next, "world", self.get_clock().now().to_msg()) + self.posestamped_publisher_.publish(posestamped_msg) + + self.yaw_publisher_.publish(Float32(data=x_next[2])) + + # Pub Paths + self.state_path.header.frame_id = "world" + self.state_path.header.stamp = self.get_clock().now().to_msg() + + self.update_path_element(self.state_path, posestamped_msg, self.N) + self.state_path_publisher_.publish(self.state_path) + + xref_msg = state_to_posestamped(self.x_ref, "world", self.get_clock().now().to_msg()) + self.xref_path.header.frame_id = "world" + self.xref_path.header.stamp = self.get_clock().now().to_msg() + + self.update_path_element(self.xref_path, xref_msg, self.N) + self.xref_path_publisher_.publish(self.xref_path) + + # Pub frame and odom tf + self.tf_broadcaster.fixed_frame_broadcaster() + self.tf_broadcaster.handle_pose(x_next) + + # Update current sim step + self.num_steps_simulated += 1 + + def plot_matplotlib(self): + state_x = [pose.pose.position.x for pose in self.state_path.poses] + state_y = [pose.pose.position.y for pose in self.state_path.poses] + ref_x = [pose.pose.position.x for pose in self.xref_path.poses] + ref_y = [pose.pose.position.y for pose in self.xref_path.poses] + + time = np.linspace(0, self.T, len(state_x)) + tau_time = np.linspace(0, self.T, len(self.tau_history)) + + plt.figure() + plt.plot(state_x, state_y, label='State Path') + plt.plot(ref_x, ref_y, label='Reference Path', linestyle='--') + plt.title('ASV Path') + plt.xlabel('X') + plt.ylabel('Y') + plt.grid() + plt.legend() + + plt.figure() + plt.plot(time, state_x, label='Actual x') + plt.plot(time, ref_x, label='Reference x') + plt.plot(time, state_y, label='Actual y') + plt.plot(time, ref_y, label='Reference y') + plt.title('Actual position vs reference position') + plt.xlabel('Time [s]') + plt.ylabel('Position [m]') + plt.gca().set_axisbelow(True) + plt.grid() + plt.legend() + + plt.figure() + plt.title('Control input values tau') + plt.subplot(2, 1, 1) + plt.plot(tau_time, self.tau_history[:, 0], label='Surge force') + plt.xlabel('Time [s]') + plt.ylabel('Force [N]') + plt.grid() + plt.legend() + plt.subplot(2, 1, 2) + plt.plot(tau_time, self.tau_history[:, 1], label='Sway force', color='red') + plt.xlabel('Time [s]') + plt.ylabel('Force [N]') + plt.grid() + plt.legend() + + plt.show() + +def main(args=None): + rclpy.init(args=args) + node = ASVSimulatorNode() + rclpy.spin(node) + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/simulation/asv_simulator/asv_simulator/conversions.py b/simulation/asv_simulator/asv_simulator/conversions.py new file mode 100644 index 00000000..92f14cfb --- /dev/null +++ b/simulation/asv_simulator/asv_simulator/conversions.py @@ -0,0 +1,59 @@ +import numpy as np + +from transforms3d.euler import euler2quat, quat2euler +from nav_msgs.msg import Odometry +from geometry_msgs.msg import PoseStamped + + +def odometrymsg_to_state(msg): + x = msg.pose.pose.position.x + y = msg.pose.pose.position.y + orientation_q = msg.pose.pose.orientation + orientation_list = [ + orientation_q.w, orientation_q.x, orientation_q.y, orientation_q.z + ] + + # Convert quaternion to Euler angles, extract yaw + yaw = quat2euler(orientation_list)[2] + + vx = msg.twist.twist.linear.x + vy = msg.twist.twist.linear.y + vyaw = msg.twist.twist.angular.z + + state = np.array([x, y, yaw, vx, vy, vyaw]) + return state + +def state_to_odometrymsg(state): + orientation_list_next = euler2quat(0, 0, state[2]) + + odometry_msg = Odometry() + odometry_msg.pose.pose.position.x = state[0] + odometry_msg.pose.pose.position.y = state[1] + odometry_msg.pose.pose.position.z = 0.0 + odometry_msg.pose.pose.orientation.w = orientation_list_next[0] + odometry_msg.pose.pose.orientation.x = orientation_list_next[1] + odometry_msg.pose.pose.orientation.y = orientation_list_next[2] + odometry_msg.pose.pose.orientation.z = orientation_list_next[3] + + return odometry_msg + +def state_to_posestamped(state, frame_id, stamp): + orientation_list_next = euler2quat(0, 0, state[2]) + + posestamped_msg = PoseStamped() + + posestamped_msg.header.frame_id = frame_id + posestamped_msg.header.stamp = stamp + + posestamped_msg.pose.position.x = state[0] + posestamped_msg.pose.position.y = state[1] + posestamped_msg.pose.position.z = 0.0 + posestamped_msg.pose.orientation.w = orientation_list_next[0] + posestamped_msg.pose.orientation.x = orientation_list_next[1] + posestamped_msg.pose.orientation.y = orientation_list_next[2] + posestamped_msg.pose.orientation.z = orientation_list_next[3] + + return posestamped_msg + +def ssa(angle): + return (angle + np.pi) % (2 * np.pi) - np.pi \ No newline at end of file diff --git a/simulation/asv_simulator/asv_simulator/simulation.py b/simulation/asv_simulator/asv_simulator/simulation.py new file mode 100644 index 00000000..2224d261 --- /dev/null +++ b/simulation/asv_simulator/asv_simulator/simulation.py @@ -0,0 +1,37 @@ +import numpy as np + +def state_dot(M_inv: np.ndarray, D: np.ndarray, state: np.ndarray, tau_actuation: np.ndarray, V_current: np.ndarray = np.zeros(2)) -> np.ndarray: + """ + Calculate the derivative of the state using the non-linear kinematics + """ + heading = state[2] + + J = np.array( + [[np.cos(heading), -np.sin(heading), 0], + [np.sin(heading), np.cos(heading), 0], + [0, 0, 1]] + ) + + A = np.zeros((6,6)) + + A[:3,3:] = J + A[3:, 3:] = - M_inv @ D + + B = np.zeros((6,3)) + B[3:,:] = M_inv + + x_dot = A @ state + B @ tau_actuation + x_dot[0:2] += V_current # add current drift term at velocity level + + return x_dot + +def RK4_integration_step(M_inv: np.ndarray, D: np.ndarray, x: np.ndarray, u: np.ndarray, dt: float) -> np.ndarray: + # integration scheme for simulation, implements the Runge-Kutta 4 integrator + k1 = state_dot(M_inv, D, x, u) + k2 = state_dot(M_inv, D, x+dt/2*k1, u) + k3 = state_dot(M_inv, D, x+dt/2*k2, u) + k4 = state_dot(M_inv, D, x+dt*k3, u) + + x_next = x + dt/6*(k1+2*k2+2*k3+k4) + + return x_next \ No newline at end of file diff --git a/simulation/asv_simulator/config/asv_sim_config.yaml b/simulation/asv_simulator/config/asv_sim_config.yaml new file mode 100644 index 00000000..6820def0 --- /dev/null +++ b/simulation/asv_simulator/config/asv_sim_config.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + asv_sim: + plot_matplotlib_enabled: true + progress_bar_enabled: true + hybridpath_path_following: false + T: 200 + waypoints: [0.0, 0.0, 5.0, 5.0, 5.0, 10.0] # [x1, y1, x2, y2, x3, y3] \ No newline at end of file diff --git a/simulation/asv_simulator/config/freya_sim_foxglove_layout.json b/simulation/asv_simulator/config/freya_sim_foxglove_layout.json new file mode 100644 index 00000000..602ac170 --- /dev/null +++ b/simulation/asv_simulator/config/freya_sim_foxglove_layout.json @@ -0,0 +1,214 @@ +{ + "configById": { + "3D!45bpewl": { + "cameraState": { + "perspective": true, + "distance": 250.52738551722237, + "phi": 179.99994266987844, + "thetaOffset": -89.99999770360385, + "targetOffset": [ + 73.97173196727483, + 0.23675590015567488, + 1.2349479935931187e-21 + ], + "target": [ + 0, + 0, + 0 + ], + "targetOrientation": [ + 0, + 0, + 0, + 1 + ], + "fovy": 45, + "near": 0.5, + "far": 5000 + }, + "followMode": "follow-pose", + "followTf": "world", + "scene": { + "transforms": { + "lineWidth": 0.5 + }, + "meshUpAxis": "y_up" + }, + "transforms": {}, + "topics": { + "/sensor/seapath/path/ned": { + "visible": true, + "lineWidth": 0.5, + "gradient": [ + "#ffffffff", + "#00be1080" + ] + }, + "/sensor/seapath/posestamped/ned": { + "visible": true, + "axisScale": 10.2, + "type": "axis", + "arrowScale": [ + 10, + 3.15, + 3.15 + ] + }, + "/move_base_simple/goal": { + "visible": false + }, + "/sensor/seapath/state_path/ned": { + "visible": true, + "lineWidth": 1, + "gradient": [ + "#fafafaff", + "#00ff0d80" + ] + }, + "/sensor/seapath/xref_path/ned": { + "visible": true, + "gradient": [ + "#c750cfff", + "#ae19a9ff" + ], + "lineWidth": 0.5 + } + }, + "layers": { + "6bba1d8c-b741-42b0-901c-20e59bfe5fcc": { + "visible": true, + "frameLocked": true, + "label": "Grid", + "instanceId": "6bba1d8c-b741-42b0-901c-20e59bfe5fcc", + "layerId": "foxglove.Grid", + "size": 1000, + "divisions": 100, + "color": "#248eff", + "position": [ + 0, + 0, + 0 + ], + "rotation": [ + 0, + 0, + 0 + ], + "order": 1, + "lineWidth": 0.3 + } + }, + "publish": { + "type": "point", + "poseTopic": "/move_base_simple/goal", + "pointTopic": "/clicked_point", + "poseEstimateTopic": "/initialpose", + "poseEstimateXDeviation": 0.5, + "poseEstimateYDeviation": 0.5, + "poseEstimateThetaDeviation": 0.26179939 + }, + "imageMode": {} + }, + "Plot!41trey": { + "paths": [ + { + "timestampMethod": "receiveTime", + "value": "/sensor/seapath/posestamped/ned.pose.position.x", + "enabled": true + }, + { + "timestampMethod": "receiveTime", + "value": "/controller/lqr/reference.pose.pose.position.x", + "enabled": true + } + ], + "showXAxisLabels": true, + "showYAxisLabels": true, + "showLegend": false, + "legendDisplay": "floating", + "showPlotValuesInLegend": true, + "isSynced": true, + "xAxisVal": "timestamp", + "sidebarDimension": 240, + "followingViewWidth": 30, + "foxglovePanelTitle": "Surge" + }, + "Plot!q6f7ui": { + "paths": [ + { + "timestampMethod": "receiveTime", + "value": "/sensor/seapath/posestamped/ned.pose.position.y", + "enabled": true + }, + { + "timestampMethod": "receiveTime", + "value": "/controller/lqr/reference.pose.pose.position.y", + "enabled": true + } + ], + "showXAxisLabels": true, + "showYAxisLabels": true, + "showLegend": false, + "legendDisplay": "floating", + "showPlotValuesInLegend": true, + "isSynced": true, + "xAxisVal": "timestamp", + "sidebarDimension": 240, + "followingViewWidth": 30, + "foxglovePanelTitle": "Sway" + }, + "Tab!23gzpt8": { + "activeTabIdx": 0, + "tabs": [ + { + "title": "1", + "layout": { + "first": "3D!45bpewl", + "second": { + "first": "Plot!41trey", + "second": { + "first": "Plot!q6f7ui", + "second": "Plot!4ia1dcm", + "direction": "row" + }, + "direction": "row", + "splitPercentage": 29.72972972972973 + }, + "direction": "column", + "splitPercentage": 69.86721144024514 + } + } + ] + }, + "Plot!4ia1dcm": { + "paths": [ + { + "timestampMethod": "receiveTime", + "value": "/sensor/seapath/yaw/ned.data", + "enabled": true + }, + { + "timestampMethod": "receiveTime", + "value": "/sensor/seapath/yaw_ref/ned.data", + "enabled": true + } + ], + "showXAxisLabels": true, + "showYAxisLabels": true, + "showLegend": true, + "legendDisplay": "floating", + "showPlotValuesInLegend": true, + "isSynced": true, + "xAxisVal": "timestamp", + "sidebarDimension": 240, + "followingViewWidth": 30, + "foxglovePanelTitle": "Yaw" + } + }, + "globalVariables": {}, + "userNodes": {}, + "playbackConfig": { + "speed": 1 + }, + "layout": "Tab!23gzpt8" +} \ No newline at end of file diff --git a/simulation/asv_simulator/launch/asv_simulator.launch.py b/simulation/asv_simulator/launch/asv_simulator.launch.py new file mode 100644 index 00000000..77cbe70a --- /dev/null +++ b/simulation/asv_simulator/launch/asv_simulator.launch.py @@ -0,0 +1,20 @@ +import os +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + asv_simulator_node = Node( + package='asv_simulator', + executable='asv_simulator_node.py', + name='asv_simulator_node', + output='screen', + parameters=[ + os.path.join(get_package_share_directory('asv_setup'), 'config', 'robots', 'freya.yaml'), + os.path.join(get_package_share_directory('asv_simulator'), 'config', 'asv_sim_config.yaml')] + ) + + return LaunchDescription([ + asv_simulator_node + ]) diff --git a/simulation/asv_simulator/package.xml b/simulation/asv_simulator/package.xml new file mode 100644 index 00000000..52befdf9 --- /dev/null +++ b/simulation/asv_simulator/package.xml @@ -0,0 +1,28 @@ + + + + asv_simulator + 0.0.0 + This package provides the implementation of the asv_simulator for the Vortex ASV. + vortex + MIT + + ament_cmake_python + + rclpy + python-transforms3d-pip + nav_msgs + geometry_msgs + vortex_msgs + std_msgs + numpy + matplotlib + scipy + + python3-pytest + + + + ament_cmake + + \ No newline at end of file