diff --git a/asv_setup/CMakeLists.txt b/asv_setup/CMakeLists.txt deleted file mode 100644 index d57de63a..00000000 --- a/asv_setup/CMakeLists.txt +++ /dev/null @@ -1,24 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(asv_setup) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -find_package(ament_cmake REQUIRED) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - -# Install launch files. -install(DIRECTORY - config - launch - DESTINATION share/${PROJECT_NAME}/ -) - -ament_package() diff --git a/asv_setup/README.md b/asv_setup/README.md deleted file mode 100644 index e0664cd3..00000000 --- a/asv_setup/README.md +++ /dev/null @@ -1,14 +0,0 @@ -## ASV setup - -This package contains global configs and the main launchfiles. The reason for putting these in a separate package is to be able to reach the aforementioned files from anywhere in a simple manner. - -### Config -The config folder contains physical parameters related to the AUV and the environment - -There are currently no physical parameters - -### Launch -This package contains a launchfile for each specific ASV. Additionally the shoreside.launch.py file is to -be used on the topside computer that the joystick is connected to, for ROV operations. - - diff --git a/asv_setup/config/environments/trondheim_freshwater.yaml b/asv_setup/config/environments/trondheim_freshwater.yaml deleted file mode 100644 index 59c915c9..00000000 --- a/asv_setup/config/environments/trondheim_freshwater.yaml +++ /dev/null @@ -1,13 +0,0 @@ -# Environmental parameters for freshwater in Trondheim, Norway -# -# Pressure value from 10 year average via wolframalpha.com -# Gravity value from EGM2008 12th order via wolframalpha.com - -atmosphere: - pressure: 100000 # phone measurement from mclab - -water: - density: 1000 - -gravity: - acceleration: 9.82841 diff --git a/asv_setup/config/environments/trondheim_saltwater.yaml b/asv_setup/config/environments/trondheim_saltwater.yaml deleted file mode 100644 index 7b85ec50..00000000 --- a/asv_setup/config/environments/trondheim_saltwater.yaml +++ /dev/null @@ -1,14 +0,0 @@ -# Environmental parameters for saltwater in Trondheim, Norway -# -# Pressure value from 10 year average via wolframalpha.com -# Density from wolgframalpha.com query "ocean density" (default parameters) -# Gravity value from EGM2008 12th order via wolframalpha.com - -atmosphere: - pressure: 101000 # [Pa] - -water: - density: 1026 # [kg/m3] - -gravity: - acceleration: 9.82841 diff --git a/asv_setup/config/robots/freya.yaml b/asv_setup/config/robots/freya.yaml deleted file mode 100644 index cc960e68..00000000 --- a/asv_setup/config/robots/freya.yaml +++ /dev/null @@ -1,75 +0,0 @@ -# This file defines parameters specific to Freya -# -# When looking at the ASV from above, the thruster placement is: -# /\ /\ -# / \ front / \ -# / \ / \ -# |=3↗=| |=0↖=| -# | | | | -# | | | | -# | |======| | -# | | | | -# | | | | -# | | | | -# |=2↖=|==||==|=1↗=| -# -/**: - ros__parameters: - physical: - mass_kg: 0 - displacement_m3: 0 - buoyancy: 0 # kg - center_of_mass: [0, 0, 0] # mm (x,y,z) - center_of_buoyancy: [0, 0, 0] # mm (x,y,z) - - - propulsion: - dofs: - num: 3 - which: - surge: true - sway: true - yaw: true - thrusters: - num: 4 - min: -200 - max: 200 - configuration_matrix: #NED - [0.70711, 0.70711, 0.70711, 0.70711, - -0.70711, 0.70711, -0.70711, 0.70711, - -0.8485, -0.8485, 0.8485, 0.8485] - - rate_of_change: - max: 0 # Maximum rate of change in newton per second for a thruster - - thruster_to_pin_mapping: [3, 2, 1, 0] # I.e. if thruster_to_pin = [1, 3, 2, 0] then thruster 0 is pin 1 etc.. - thruster_direction: [-1, 1, 1,-1] # Disclose during thruster mapping (+/- 1) - thruster_PWM_offset: [0, 0, 0, 0] # Disclose during thruster mapping, Recomended: [0, 0, 0, 0] - thruster_PWM_min: [1100, 1100, 1100, 1100] # Minimum PWM value, Recomended: [1100, 1100, 1100, 1100] - thruster_PWM_max: [1900, 1900, 1900, 1900] # Maximum PWM value, Recomended: [1900, 1900, 1900, 1900] - command: - wrench: - max: [150.0, 150.0, 150.0, 150.0, 150.0, 150.0] - scaling: [ 1.0, 1.0, 1.0, 1.0, 1.0, 1.0] - - blackbox: - data_logging_rate: 5.0 # [logings/second], Recomended: 5.0 logings per second - - internal_status: - psm_read_rate: 2.0 # [readings/second], Recomended 2.0 readings per second OBS! Try keeping under 2.0 as otherwise I2C comunication to Power Sense Module might become bad as psm needs time to sample data - - acoustics: - frequencies_of_interest: # MUST be 20 elements [FREQUENCY [Hz], FREQUENCY_VARIANCE [Hz]...] Recoended: Frequency 1 000 - 50 000 Hz, Variance 1 000 - 10 000 - [10000, 1000, - 50000, 3000, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0] - data_logging_rate: 1.0 # [logings/second], Recomended: 1.0 logings per second - - computer: "pc-debug" diff --git a/asv_setup/launch/ASV_simulator.launch.py b/asv_setup/launch/ASV_simulator.launch.py deleted file mode 100644 index a68b94c7..00000000 --- a/asv_setup/launch/ASV_simulator.launch.py +++ /dev/null @@ -1,35 +0,0 @@ -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(): - # LQR_controller launch - lqr_controller_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory('lqr_controller'), 'launch/lqr_controller.launch.py') - ) - ) - - # LOS_guidance launch - los_guidance_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory('los_guidance'), 'launch/los_guidance.launch.py') - ) - ) - - # Vessel_simulator launch - vessel_simulator_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory('vessel_simulator'), 'launch/vessel_simulator.launch.py') - ) - ) - - # Return launch description - return LaunchDescription([ - lqr_controller_launch, - los_guidance_launch, - vessel_simulator_launch - ]) \ No newline at end of file diff --git a/asv_setup/launch/freya.launch.py b/asv_setup/launch/freya.launch.py deleted file mode 100644 index f9b3ec16..00000000 --- a/asv_setup/launch/freya.launch.py +++ /dev/null @@ -1,34 +0,0 @@ -from os import path -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(): - # Set environment variable - set_env_var = SetEnvironmentVariable( - name='ROSCONSOLE_FORMAT', - value='[${severity}] [${time}] [${node}]: ${message}' - ) - - # Thruster Allocator launch - thruster_allocator_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - path.join(get_package_share_directory('thruster_allocator'),'launch','thruster_allocator.launch.py') - ) - ) - - #Thruster Interface launch - thruster_interface_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - path.join(get_package_share_directory('thruster_interface_asv'),'launch','thruster_interface_asv.launch.py') - ) - ) - - # Return launch description - return LaunchDescription([ - set_env_var, - thruster_allocator_launch, - thruster_interface_launch - ]) diff --git a/asv_setup/launch/hybridpath.launch.py b/asv_setup/launch/hybridpath.launch.py deleted file mode 100644 index 637029d9..00000000 --- a/asv_setup/launch/hybridpath.launch.py +++ /dev/null @@ -1,25 +0,0 @@ -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/asv_setup/launch/shoreside.launch.py b/asv_setup/launch/shoreside.launch.py deleted file mode 100644 index 98c62f71..00000000 --- a/asv_setup/launch/shoreside.launch.py +++ /dev/null @@ -1,42 +0,0 @@ -import os -from launch import LaunchDescription -from launch.actions import SetEnvironmentVariable, IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch_ros.actions import Node -from ament_index_python.packages import get_package_share_directory - -def generate_launch_description(): - # Set environment variable - set_env_var = SetEnvironmentVariable( - name='ROSCONSOLE_FORMAT', - value='[${severity}] [${time}] [${node}]: ${message}' - ) - - # Joystick node - joy_node = Node( - package='joy', - executable='joy_node', - name='joystick_driver', - output='screen', - parameters=[ - {'deadzone': 0.15}, - {'autorepeat_rate': 100.0}, - ], - remappings=[ - ('/joy', '/joystick/joy'), - ], - ) - - # Joystick interface launch - joystick_interface_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory('joystick_interface'), 'launch/joystick_interface.launch.py') - ) - ) - - # Return launch description - return LaunchDescription([ - set_env_var, - joy_node, - joystick_interface_launch - ]) diff --git a/asv_setup/launch/tf.launch.py b/asv_setup/launch/tf.launch.py deleted file mode 100644 index 47a7de37..00000000 --- a/asv_setup/launch/tf.launch.py +++ /dev/null @@ -1,81 +0,0 @@ -import math - -from launch import LaunchDescription -from launch_ros.actions import Node -from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSDurabilityPolicy, QoSHistoryPolicy -from launch.actions import ExecuteProcess - - -def generate_launch_description(): - # command line arguments to get desired QoS settings - echo_tf_static = ExecuteProcess( - cmd=['ros2', 'topic', 'echo', '--qos-durability', 'transient_local', '--qos-reliability', 'reliable', '/tf_static'], - output='screen' - ) - - # base_link (NED) to base_link (SEU) tf. - tf_base_link_ned_to_base_link_seu = Node( - package='tf2_ros', - name='base_link_ned_to_base_link_seu', - executable='static_transform_publisher', - arguments=['--x' , '0', - '--y' , '0', - '--z' , '0', - '--roll' , '0', - '--pitch' , str(math.pi), - '--yaw' , '0', - '--frame-id' , 'base_link', - '--child-frame-id' , 'base_link_SEU'], - ) - - tf_base_link_to_lidar = Node( - package='tf2_ros', - name='base_link_to_lidar', - executable='static_transform_publisher', - arguments=['--x' , '-0.0145', - '--y' , '0', - '--z' , '-0.392425', - '--roll' , str(math.pi), - '--pitch' , '0', - '--yaw' , str(math.pi), - '--frame-id' , 'base_link', - '--child-frame-id' , 'os_sensor'], - ) - - # base_link to zed2i_camera_center tf. - tf_base_link_to_zed2_camera_center = Node( - package='tf2_ros', - name='base_link_to_zed2_camera_center', - executable='static_transform_publisher', - arguments=['--x' , '0.3005', - '--y' , '0', - '--z' , '-0.22036', - '--roll' , str(math.pi), - '--pitch' , '0', - '--yaw' , '0', - '--frame-id' , 'base_link', - '--child-frame-id' , 'zed2i_camera_center'], - ) - - # base_link (NED) to seapath_frame (NED?) tf. - tf_base_link_ned_to_seapath_frame = Node( - package='tf2_ros', - name='base_link_ned_to_seapath_frame', - executable='static_transform_publisher', - arguments=['--x' , '0', - '--y' , '0', - '--z' , '0', - '--roll' , '0', - '--pitch' , '0', - '--yaw' , '0', - '--frame-id' , 'base_link', - '--child-frame-id' , 'seapath_frame'], - ) - - return LaunchDescription([ - tf_base_link_ned_to_base_link_seu, - tf_base_link_to_lidar, - tf_base_link_to_zed2_camera_center, - tf_base_link_ned_to_seapath_frame, - echo_tf_static - ]) diff --git a/asv_setup/package.xml b/asv_setup/package.xml deleted file mode 100644 index 97057113..00000000 --- a/asv_setup/package.xml +++ /dev/null @@ -1,18 +0,0 @@ - - - - asv_setup - 0.0.0 - The asv_setup package - alekskl01 - MIT - - ament_cmake - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/mission/mission_planner/CMakeLists.txt b/mission/mission_planner/CMakeLists.txt deleted file mode 100644 index 827ef381..00000000 --- a/mission/mission_planner/CMakeLists.txt +++ /dev/null @@ -1,32 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(mission_planner) - -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) - -ament_python_install_package(${PROJECT_NAME}) - -install(DIRECTORY - launch - DESTINATION share/${PROJECT_NAME} -) - -install(PROGRAMS - mission_planner/mission_planner.py - DESTINATION lib/${PROJECT_NAME} -) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - -ament_package() diff --git a/mission/mission_planner/launch/mission_planner.launch.py b/mission/mission_planner/launch/mission_planner.launch.py deleted file mode 100644 index c80e0391..00000000 --- a/mission/mission_planner/launch/mission_planner.launch.py +++ /dev/null @@ -1,17 +0,0 @@ -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(): - - mission_planner_node = Node( - package='mission_planner', - executable='mission_planner.py', - name='mission_planner', - output='screen' - ) - - return LaunchDescription([ - mission_planner_node - ]) \ No newline at end of file diff --git a/mission/mission_planner/mission_planner/__init__.py b/mission/mission_planner/mission_planner/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/mission/mission_planner/mission_planner/mission_planner.py b/mission/mission_planner/mission_planner/mission_planner.py deleted file mode 100755 index f856fc46..00000000 --- a/mission/mission_planner/mission_planner/mission_planner.py +++ /dev/null @@ -1,88 +0,0 @@ -#!/usr/bin/env python3 - -<<<<<<< HEAD -import sys -======= ->>>>>>> development -import rclpy -from rclpy.node import Node -from vortex_msgs.srv import MissionParameters -from geometry_msgs.msg import Point - -class MissionPlannerClient(Node): - """ - A ROS2 client node for interacting with the MissionPlanner service. - """ - def __init__(self): - """ - Initializes the client node, creates a client for the MissionPlanner service, - and waits for the service to become available. - """ - super().__init__('mission_planner_client') - self.client = self.create_client(MissionParameters, 'mission_parameters') - while not self.client.wait_for_service(timeout_sec=1.0): - self.get_logger().info('Service not available, waiting again...') - self.req = MissionParameters.Request() - - def send_request(self, obstacles: list[Point], start: Point, goal: Point, origin: Point, height: int, width: int): - """ - Sends an asynchronous request to the MissionPlanner service with obstacle locations, - start, and goal positions. - - Args: - obstacles (list[Point]): The list of obstacle points. - start (Point): The start point. - goal (Point): The goal point. - origin (Point): The origin point of the world. - height (int): The height of the world. - width (int): The width of the world. - """ - self.req.obstacles = obstacles - self.req.start = start - self.req.goal = goal - self.req.origin = origin - self.req.height = height - self.req.width = width - - self.future = self.client.call_async(self.req) - self.get_logger().info('MissionPlanner service has been called') - -def main(args=None): - """ - Main function with example usage of the MissionPlannerClient. - - """ - 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) - if mission_planner_client.future.done(): - try: - response = mission_planner_client.future.result() - mission_planner_client.get_logger().info(f'Success: {response.success}') - except Exception as e: - mission_planner_client.get_logger().info(f'Service call failed {e}') - break # Break out of the loop if the service call failed - else: - # This else block should be executed if no exceptions were raised - mission_planner_client.get_logger().info('Successfully created path') - break # Ensure to break out of the loop once processing is done - rclpy.shutdown() - -if __name__ == '__main__': - main() - - \ No newline at end of file diff --git a/mission/mission_planner/package.xml b/mission/mission_planner/package.xml deleted file mode 100644 index 3906016a..00000000 --- a/mission/mission_planner/package.xml +++ /dev/null @@ -1,20 +0,0 @@ - - - - mission_planner - 0.0.0 - Mission planner for path planning - andeshog - MIT - - ament_lint_auto - ament_lint_common - - rclpy - std_msgs - vortex_msgs - - - ament_cmake - - diff --git a/motion/hybridpath_controller/README.md b/motion/hybridpath_controller/README.md deleted file mode 100755 index 2159398d..00000000 --- a/motion/hybridpath_controller/README.md +++ /dev/null @@ -1,199 +0,0 @@ -# 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} -``` \ 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 deleted file mode 100644 index dcf4f422..00000000 --- a/motion/hybridpath_controller/config/hybridpath_controller_config.yaml +++ /dev/null @@ -1 +0,0 @@ -# K1 and K2 should be put here \ No newline at end of file diff --git a/motion/hybridpath_controller/hybridpath_controller/__init__.py b/motion/hybridpath_controller/hybridpath_controller/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/motion/hybridpath_controller/hybridpath_controller/adaptive_backstep.py b/motion/hybridpath_controller/hybridpath_controller/adaptive_backstep.py deleted file mode 100644 index 74667ea5..00000000 --- a/motion/hybridpath_controller/hybridpath_controller/adaptive_backstep.py +++ /dev/null @@ -1,134 +0,0 @@ -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): - self.init_system() - - def init_system(self) -> None: - self.K_1 = np.diag([10, 10, 10]) # First gain matrix - self.K_2 = np.diag([60, 60, 30]) # Second gain matrix - self.tau_max = np.array([41.0, 50.0, 55.0]) # Må tilpasses thrusterne - - ## Forenklet modell ## Bør også endres - m = 50 - self.M = np.diag([m, m, m]) - self.D = np.diag([10, 10, 5]) - - 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.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 - - def calculate_coriolis_matrix(self, nu): # Må bestemme om dette er noe vi skal bruke - # u = nu[0] - # v = nu[1] - # r = nu[2] - - # C_RB = np.array([[0.0, 0.0, -self.m * (self.xg * r + v)], [0.0, 0.0, self.m * u], - # [self.m*(self.xg*r+v), -self.m*u, 0.0]]) - # C_A = np.array([[0.0, 0.0, -self.M_A[1,1] * v + (-self.M_A[1,2])*r],[0.0,0.0,-self.M_A[0,0]*u], - # [self.M_A[1,1]*v-(-self.M_A[1,2])*r, self.M_A[0,0]*u, 0.0]]) - # C = C_RB + C_A - - #return C - pass - - @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 deleted file mode 100644 index 4621a292..00000000 --- a/motion/hybridpath_controller/hybridpath_controller/hybridpath_controller_node.py +++ /dev/null @@ -1,58 +0,0 @@ -import rclpy -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 -from rclpy.qos import QoSProfile, qos_profile_sensor_data, QoSHistoryPolicy, QoSReliabilityPolicy - - -qos_profile = QoSProfile(depth=1, history=qos_profile_sensor_data.history, - reliability=QoSReliabilityPolicy.BEST_EFFORT) - - -class HybridPathControllerNode(Node): - def __init__(self): - super().__init__("hybridpath_controller_node") - - self.state_subscriber_ = self.state_subscriber_ = self.create_subscription(Odometry, "/sensor/seapath/odom/ned", self.state_callback, qos_profile=qos_profile) - 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) - - self.AB_controller_ = AdaptiveBackstep() - - 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 deleted file mode 100644 index 5310fb5d..00000000 --- a/motion/hybridpath_controller/launch/hybridpath_controller.launch.py +++ /dev/null @@ -1,16 +0,0 @@ -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', - name='hybridpath_controller_node', - parameters=[os.path.join(get_package_share_directory('hybridpath_controller'),'config','hybridpath_controller_config.yaml')], - output='screen', - ) - return LaunchDescription([ - hybridpath_controller_node - ]) diff --git a/motion/hybridpath_controller/package.xml b/motion/hybridpath_controller/package.xml deleted file mode 100644 index f89a6f95..00000000 --- a/motion/hybridpath_controller/package.xml +++ /dev/null @@ -1,24 +0,0 @@ - - - - hybridpath_controller - 0.0.0 - TODO: Package description - vortex - MIT - - rclpy - python-transforms3d-pip - geometry_msgs - vortex_msgs - - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - - - - ament_python - - diff --git a/motion/hybridpath_controller/resource/hybridpath_controller b/motion/hybridpath_controller/resource/hybridpath_controller deleted file mode 100644 index e69de29b..00000000 diff --git a/motion/hybridpath_controller/setup.cfg b/motion/hybridpath_controller/setup.cfg deleted file mode 100644 index daf09782..00000000 --- a/motion/hybridpath_controller/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/hybridpath_controller -[install] -install_scripts=$base/lib/hybridpath_controller diff --git a/motion/hybridpath_controller/setup.py b/motion/hybridpath_controller/setup.py deleted file mode 100644 index a0ed58ea..00000000 --- a/motion/hybridpath_controller/setup.py +++ /dev/null @@ -1,30 +0,0 @@ -import os -from glob import glob -from setuptools import find_packages, setup - -package_name = 'hybridpath_controller' - -setup( - name=package_name, - version='0.0.0', - packages=find_packages(exclude=['test']), - data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), - (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), - (os.path.join('share', package_name, 'config'), glob('config/*.yaml')), - ], - install_requires=['setuptools'], - zip_safe=True, - maintainer='vortex', - maintainer_email='vortex.git@vortexntnu.no', - description='TODO: Package description', - license='MIT', - tests_require=['pytest'], - entry_points={ - 'console_scripts': [ - 'hybridpath_controller_node = hybridpath_controller.hybridpath_controller_node:main' - ], - }, -) diff --git a/motion/hybridpath_controller/tests/test_hybridpath_controller.py b/motion/hybridpath_controller/tests/test_hybridpath_controller.py deleted file mode 100644 index 52919a77..00000000 --- a/motion/hybridpath_controller/tests/test_hybridpath_controller.py +++ /dev/null @@ -1,82 +0,0 @@ -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/LICENSE b/motion/lqr_controller/LICENSE deleted file mode 100644 index 30e8e2ec..00000000 --- a/motion/lqr_controller/LICENSE +++ /dev/null @@ -1,17 +0,0 @@ -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in -all copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -THE SOFTWARE. diff --git a/motion/lqr_controller/config/lqr_config.yaml b/motion/lqr_controller/config/lqr_config.yaml deleted file mode 100644 index 50dc3463..00000000 --- a/motion/lqr_controller/config/lqr_config.yaml +++ /dev/null @@ -1,7 +0,0 @@ -/**: - ros__parameters: - lqr_controller: - mass: 50.0 - D: [10.0, 10.0, 5.0] # Damping coefficients - Q: [10.0, 10.0, 5.0, 0.1, 1.0, 5.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 deleted file mode 100644 index b51a0ab3..00000000 --- a/motion/lqr_controller/launch/lqr_controller.launch.py +++ /dev/null @@ -1,16 +0,0 @@ -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', - name='lqr_controller_node', - parameters=[os.path.join(get_package_share_directory('lqr_controller'),'config','lqr_config.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 deleted file mode 100644 index e69de29b..00000000 diff --git a/motion/lqr_controller/lqr_controller/asv_model.py b/motion/lqr_controller/lqr_controller/asv_model.py deleted file mode 100644 index 09231ce1..00000000 --- a/motion/lqr_controller/lqr_controller/asv_model.py +++ /dev/null @@ -1,72 +0,0 @@ -import numpy as np - -class ASV(): - - def __init__(self, M: np.ndarray, D: np.ndarray): - """ - Initialize some system matrices - """ - self.M = M - self.M_inv = np.linalg.inv(self.M) - self.D = D - - def linearize_model(self, heading: float) -> tuple[np.ndarray, np.ndarray]: - """ - Get a linearization about some heading - """ - 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:] = - self.M_inv @ self.D - - B = np.zeros((6,3)) - B[3:,:] = self.M_inv - - return A, B - - def RK4_integration_step(self, x: np.ndarray, u: np.ndarray, dt: float) -> np.ndarray: - """ - Perform an integration step using the Runge-Kutta 4 integrator - """ - k1 = self.state_dot(x, u) - k2 = self.state_dot(x+dt/2*k1, u) - k3 = self.state_dot(x+dt/2*k2, u) - k4 = self.state_dot(x+dt*k3, u) - - x_next = x + dt/6*(k1+2*k2+2*k3+k4) - - - return x_next - - def state_dot(self, 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:] = - self.M_inv @ self.D - - B = np.zeros((6,3)) - B[3:,:] = self.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 - - diff --git a/motion/lqr_controller/lqr_controller/lqr_controller.py b/motion/lqr_controller/lqr_controller/lqr_controller.py deleted file mode 100644 index 2d02230d..00000000 --- a/motion/lqr_controller/lqr_controller/lqr_controller.py +++ /dev/null @@ -1,43 +0,0 @@ -import numpy as np -import matplotlib.pyplot as plt -from scipy.linalg import solve_continuous_are -from lqr_controller.asv_model import ASV - -def ssa(angle): - return (angle + np.pi) % (2 * np.pi) - np.pi - -class LQRController: - - def __init__(self, m: float, D: list[float], Q: list[float], R: list[float]): - self.heading_ref = 50*np.pi/180 - - self.M = np.diag([m, m, m]) - self.M_inv = np.linalg.inv(self.M) - self.D = np.diag(D) - - self.asv = ASV(self.M, self.D) - - self.linearize_model(self.heading_ref) - - C = np.zeros((3,6)) - C[:3, :3] = np.eye(3) - - self.Q = np.diag(Q) - self.R = np.diag(R) - 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(C@np.linalg.inv(self.B @ self.K_LQR - self.A) @ self.B) - - def linearize_model(self, heading: float): - self.A, self.B = self.asv.linearize_model(heading) - - def calculate_control_input(self, x, x_ref, K_LQR, K_r): - # print("x:", x) - # print("x_ref:", x) - # print("K_LQR:", K_LQR) - # print("K_r:", K_r) - u = -K_LQR @ x + K_r @ x_ref - return u - - - \ 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 deleted file mode 100644 index 5f180f37..00000000 --- a/motion/lqr_controller/lqr_controller/lqr_controller_node.py +++ /dev/null @@ -1,98 +0,0 @@ -import rclpy -import numpy as np -from rclpy.node import Node -from geometry_msgs.msg import Wrench -from nav_msgs.msg import Odometry -from transforms3d.euler import quat2euler -from lqr_controller.lqr_controller import LQRController - -class LQRControllerNode(Node): - def __init__(self): - super().__init__("lqr_controller_node") - - # Load parameters from lqr_config.yaml - self.declare_parameters( - namespace='', - parameters=[ - ('lqr_controller.mass', 1.0), - ('lqr_controller.D', [0.0, 0.0, 0.0]), - ('lqr_controller.Q', [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]), - ('lqr_controller.R', [0.0, 0.0, 0.0]) - ]) - - self.wrench_publisher_ = self.create_publisher(Wrench, "thrust/wrench_input", 1) - self.state_subscriber_ = self.create_subscription(Odometry, "/sensor/seapath/odometry/ned", self.state_cb, 1) - self.guidance_subscriber_ = self.create_subscription(Odometry, "controller/lqr/reference", self.guidance_cb, 1) - - m = self.get_parameter('lqr_controller.mass').get_parameter_value().double_value - D = self.get_parameter('lqr_controller.D').get_parameter_value().double_array_value - 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 - - self.get_logger().info(f"Mass: {m}") - self.get_logger().info(f"D: {D}") - self.get_logger().info(f"Q: {Q}") - self.get_logger().info(f"R: {R}") - - self.lqr_controller = LQRController(m, D, Q, R) - - # Using x, y, yaw as reference (1x3) - self.x_ref = [0, 0, 0] - self.state = [0, 0, 0, 0, 0, 0] - - self.get_logger().info("lqr_controller_node started") - - def odometrymsg_to_state(self, 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 guidance_cb(self, msg): - self.x_ref = self.odometrymsg_to_state(msg)[:3] - - #wrench = self.run_lqr_to_wrench() - #self.wrench_publisher_.publish(wrench) - - def state_cb(self, msg): - self.state = self.odometrymsg_to_state(msg) - - wrench = self.run_lqr_to_wrench() - self.wrench_publisher_.publish(wrench) - - def run_lqr_to_wrench(self): - self.lqr_controller.linearize_model(self.state[2]) - u = self.lqr_controller.calculate_control_input(self.state, self.x_ref, self.lqr_controller.K_LQR, self.lqr_controller.K_r) - - wrench = Wrench() - wrench.force.x = u[0] - wrench.force.y = u[1] - wrench.force.z = 0.0 - wrench.torque.x = 0.0 - wrench.torque.y = 0.0 - wrench.torque.z = u[2] - - return wrench - - -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 deleted file mode 100644 index b38062f7..00000000 --- a/motion/lqr_controller/package.xml +++ /dev/null @@ -1,22 +0,0 @@ - - - - lqr_controller - 0.0.0 - TODO: Package description - vortex - MIT - - rclpy - geometry_msgs - nav_msgs - - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - - - ament_python - - diff --git a/motion/lqr_controller/resource/lqr_controller b/motion/lqr_controller/resource/lqr_controller deleted file mode 100644 index e69de29b..00000000 diff --git a/motion/lqr_controller/setup.cfg b/motion/lqr_controller/setup.cfg deleted file mode 100644 index e61c4ac2..00000000 --- a/motion/lqr_controller/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/lqr_controller -[install] -install_scripts=$base/lib/lqr_controller diff --git a/motion/lqr_controller/setup.py b/motion/lqr_controller/setup.py deleted file mode 100644 index ada29567..00000000 --- a/motion/lqr_controller/setup.py +++ /dev/null @@ -1,30 +0,0 @@ -import os -from glob import glob -from setuptools import find_packages, setup - -package_name = 'lqr_controller' - -setup( - name=package_name, - version='0.0.0', - packages=find_packages(exclude=['test']), - data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), - (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), - (os.path.join('share', package_name, 'config'), glob('config/*.yaml')), - ], - install_requires=['setuptools'], - zip_safe=True, - maintainer='vortex', - maintainer_email='vortex.git@vortexntnu.no', - description='TODO: Package description', - license='MIT', - tests_require=['pytest'], - entry_points={ - 'console_scripts': [ - 'lqr_controller_node = lqr_controller.lqr_controller_node:main' - ], - }, -) diff --git a/motion/thruster_allocator/CMakeLists.txt b/motion/thruster_allocator/CMakeLists.txt deleted file mode 100644 index 3b282d0c..00000000 --- a/motion/thruster_allocator/CMakeLists.txt +++ /dev/null @@ -1,50 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(thruster_allocator) - -# Default to C++14 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) -endif() - -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 REQUIRED) -find_package(rclcpp REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(std_msgs REQUIRED) -find_package(Eigen3 REQUIRED) - -include_directories(include) - -add_executable(${PROJECT_NAME}_node - src/thruster_allocator_node.cpp - src/allocator_ros.cpp - src/pseudoinverse_allocator.cpp - ) - -ament_target_dependencies(${PROJECT_NAME}_node - rclcpp - geometry_msgs - std_msgs - Eigen3 - ) - -install( - DIRECTORY include/ - DESTINATION include -) - -install(TARGETS - ${PROJECT_NAME}_node - DESTINATION lib/${PROJECT_NAME}) - -# Install launch files. -install(DIRECTORY - launch - DESTINATION share/${PROJECT_NAME}/ -) - -ament_package() diff --git a/motion/thruster_allocator/README.md b/motion/thruster_allocator/README.md deleted file mode 100644 index 208c913c..00000000 --- a/motion/thruster_allocator/README.md +++ /dev/null @@ -1,5 +0,0 @@ -## Thruster allocator - -This package takes a thrust vector and calculates the corresponding thrust required from each individual thruster. The resulting calculation is published as a std_msgs::msg::Float32MultiArray. - -The calculation itself is based on the 'unweighted pseudoinverse-based allocator' as described in Fossen (2011): Handbook of Marine Craft Hydrodynamics and Motion Control (chapter 12.3.2). \ No newline at end of file diff --git a/motion/thruster_allocator/include/thruster_allocator/allocator_ros.hpp b/motion/thruster_allocator/include/thruster_allocator/allocator_ros.hpp deleted file mode 100644 index 4aed04cd..00000000 --- a/motion/thruster_allocator/include/thruster_allocator/allocator_ros.hpp +++ /dev/null @@ -1,64 +0,0 @@ -/** - * @file allocator_ros.hpp - * @brief ThrusterAllocator class, which - * allocates thrust to the ASV's thrusters based on the desired body frame - * forces. - */ - -#ifndef VORTEX_ALLOCATOR_ALLOCATOR_ROS_HPP -#define VORTEX_ALLOCATOR_ALLOCATOR_ROS_HPP - -#include "thruster_allocator/allocator_utils.hpp" -#include "thruster_allocator/pseudoinverse_allocator.hpp" -#include -#include - -#include -#include - -using namespace std::chrono_literals; - -class ThrusterAllocator : public rclcpp::Node { -public: - explicit ThrusterAllocator(); - - /** - * @brief Calculates the allocated - * thrust based on the body frame forces. It then saturates the output vector - * between min and max values and publishes the thruster forces to the topic - * "thrust/thruster_forces". - */ - void timer_callback(); - -private: - Eigen::MatrixXd thrust_configuration; - /** - * @brief Callback function for the wrench input subscription. Extracts the - * surge, sway and yaw values from the received wrench msg - * and stores them in the body_frame_forces_ Eigen vector. - * @param msg The received geometry_msgs::msg::Wrench message. - */ - void wrench_callback(const geometry_msgs::msg::Wrench &msg); - - /** - * @brief Checks if the given Eigen vector contains any NaN or Inf values - * @param v The Eigen vector to check. - * @return True if the vector is healthy, false otherwise. - */ - bool healthyWrench(const Eigen::VectorXd &v) const; - rclcpp::Publisher::SharedPtr - thrust_publisher_; - rclcpp::Subscription::SharedPtr - wrench_subscriber_; - rclcpp::TimerBase::SharedPtr timer_; - size_t count_; - int num_dof_; - int num_thrusters_; - int min_thrust_; - int max_thrust_; - Eigen::Vector3d body_frame_forces_; - std::vector direction_; - PseudoinverseAllocator pseudoinverse_allocator_; -}; - -#endif // VORTEX_ALLOCATOR_ALLOCATOR_ROS_HPP diff --git a/motion/thruster_allocator/include/thruster_allocator/allocator_utils.hpp b/motion/thruster_allocator/include/thruster_allocator/allocator_utils.hpp deleted file mode 100644 index f36a4b49..00000000 --- a/motion/thruster_allocator/include/thruster_allocator/allocator_utils.hpp +++ /dev/null @@ -1,114 +0,0 @@ -/** - * @file allocator_utils.hpp - * @brief This file contains utility functions for the thruster allocator - * module. - */ - -#ifndef VORTEX_ALLOCATOR_ALLOCATOR_UTILS_HPP -#define VORTEX_ALLOCATOR_ALLOCATOR_UTILS_HPP - -#include -#include -#include -#include - -#include - -/** - * @brief Check if the matrix has any NaN or INF elements. - * - * @tparam Derived The type of the matrix. - * @param M The matrix to check. - * @return true if the matrix has any NaN or INF elements, false otherwise. - */ -template -inline bool isInvalidMatrix(const Eigen::MatrixBase &M) { - bool has_nan = !(M.array() == M.array()).all(); - bool has_inf = M.array().isInf().any(); - return has_nan || has_inf; -} - -/** - * @brief Returns a string stream containing the matrix with the given name. - * - * @param name The name of the matrix. - * @param M The matrix to print. - * @return std::stringstream The string stream containing the matrix. - */ -inline std::stringstream printMatrix(std::string name, - const Eigen::MatrixXd &M) { - std::stringstream ss; - ss << std::endl << name << " = " << std::endl << M; - return ss; -} - -/** - * @brief Calculates the right pseudoinverse of the given matrix. - * - * @param M The matrix to calculate the pseudoinverse of. - * @throws char* if the pseudoinverse is invalid. - * @return The pseudoinverse of the given matrix. - */ -inline Eigen::MatrixXd calculateRightPseudoinverse(const Eigen::MatrixXd &T) { - Eigen::MatrixXd pseudoinverse = T.transpose() * (T * T.transpose()).inverse(); - // pseudoinverse.completeOrthogonalDecomposition().pseudoInverse(); - if (isInvalidMatrix(pseudoinverse)) { - throw std::runtime_error("Invalid Psuedoinverse Calculated"); - } - return pseudoinverse; -} - -/** - * @brief Saturates the values of a given Eigen vector between a minimum and - * maximum value. - * - * @param vec The Eigen vector to be saturated. - * @param min The minimum value to saturate the vector values to. - * @param max The maximum value to saturate the vector values to. - * @return True if all vector values are within the given range, false - * otherwise. - */ -inline bool saturateVectorValues(Eigen::VectorXd &vec, double min, double max) { - bool all_values_in_range = - std::all_of(vec.begin(), vec.end(), - [min, max](double val) { return val >= min && val <= max; }); - - std::transform(vec.begin(), vec.end(), vec.begin(), [min, max](double val) { - return std::min(std::max(val, min), max); - }); - - return all_values_in_range; -} - -/** - * @brief Converts an Eigen VectorXd to a std_msgs::msg::Float32MultiArray - * message. - * - * @param u The Eigen VectorXd to be converted. - * @param msg The std_msgs::msg::Float32MultiArray message to store the - * converted values. - */ -#include - -inline void arrayEigenToMsg(const Eigen::VectorXd &u, - std_msgs::msg::Float32MultiArray &msg) { - msg.data.assign(u.data(), u.data() + u.size()); -} - -/** - * @brief Converts a 1D array of doubles to a 2D Eigen matrix. - * - * @param matrix The 1D array of doubles to be converted. - * @param rows The number of rows in the resulting Eigen matrix. - * @param cols The number of columns in the resulting Eigen matrix. - * @return The resulting Eigen matrix. - */ -inline Eigen::MatrixXd -doubleArrayToEigenMatrix(const std::vector &matrix, int rows, - int cols) { - return Eigen::Map>(matrix.data(), rows, - cols); -} - -#endif // VORTEX_ALLOCATOR_ALLOCATOR_UTILS_HPP diff --git a/motion/thruster_allocator/include/thruster_allocator/pseudoinverse_allocator.hpp b/motion/thruster_allocator/include/thruster_allocator/pseudoinverse_allocator.hpp deleted file mode 100644 index 0d94596e..00000000 --- a/motion/thruster_allocator/include/thruster_allocator/pseudoinverse_allocator.hpp +++ /dev/null @@ -1,39 +0,0 @@ -/** - * @file pseudoinverse_allocator.hpp - * @brief Contains the PseudoinverseAllocator class, which implements the - * unweighted pseudoinverse-based allocator described in e.g. Fossen 2011 - * Handbook of Marine Craft Hydrodynamics and Motion Control (chapter 12.3.2). - */ - -#ifndef VORTEX_ALLOCATOR_PSEUDOINVERSE_ALLOCATOR_HPP -#define VORTEX_ALLOCATOR_PSEUDOINVERSE_ALLOCATOR_HPP - -#include - -/** - * @brief The PseudoinverseAllocator class calculates the allocated thrust given - * the input torques using the pseudoinverse allocator. - */ -class PseudoinverseAllocator { -public: - /** - * @brief Constructor for the PseudoinverseAllocator class. - * - * @param T_pinv The pseudoinverse of the thruster configuration matrix. - */ - explicit PseudoinverseAllocator(const Eigen::MatrixXd &T_pinv); - - /** - * @brief Calculates the allocated thrust given the input torques using the - * pseudoinverse allocator. - * - * @param tau The input torques as a vector. - * @return The allocated thrust as a vector. - */ - Eigen::VectorXd calculateAllocatedThrust(const Eigen::VectorXd &tau); - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - Eigen::MatrixXd T_pinv; -}; - -#endif // VORTEX_ALLOCATOR_PSEUDOINVERSE_ALLOCATOR_HPP \ No newline at end of file diff --git a/motion/thruster_allocator/launch/thruster_allocator.launch.py b/motion/thruster_allocator/launch/thruster_allocator.launch.py deleted file mode 100755 index 6f3e8615..00000000 --- a/motion/thruster_allocator/launch/thruster_allocator.launch.py +++ /dev/null @@ -1,19 +0,0 @@ -from os import path -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch_ros.actions import Node - -def generate_launch_description(): - config = path.join(get_package_share_directory('asv_setup'),'config','robots','freya.yaml') - - thruster_allocator_node = Node( - package='thruster_allocator', - executable='thruster_allocator_node', - name='thruster_allocator_node', - parameters=[config], - output='screen' - ) - - return LaunchDescription([ - thruster_allocator_node - ]) diff --git a/motion/thruster_allocator/package.xml b/motion/thruster_allocator/package.xml deleted file mode 100644 index 6f3f3a38..00000000 --- a/motion/thruster_allocator/package.xml +++ /dev/null @@ -1,24 +0,0 @@ - - - - thruster_allocator - 0.0.0 - Thurster allocator for ASV - martin - MIT - - rclcpp - geometry_msgs - vortex_msgs - eigen - - ament_cmake_gtest - ament_lint_auto - ament_lint_common - - ament_cmake - - - ament_cmake - - diff --git a/motion/thruster_allocator/src/allocator_ros.cpp b/motion/thruster_allocator/src/allocator_ros.cpp deleted file mode 100644 index 3f26b0a5..00000000 --- a/motion/thruster_allocator/src/allocator_ros.cpp +++ /dev/null @@ -1,90 +0,0 @@ -#include "thruster_allocator/allocator_ros.hpp" -#include "thruster_allocator/allocator_utils.hpp" -#include "thruster_allocator/pseudoinverse_allocator.hpp" -#include - -#include -#include - -using namespace std::chrono_literals; - -ThrusterAllocator::ThrusterAllocator() - : Node("thruster_allocator_node"), - pseudoinverse_allocator_(Eigen::MatrixXd::Zero(3, 4)) { - declare_parameter("propulsion.dofs.num", 3); - declare_parameter("propulsion.thrusters.num", 4); - declare_parameter("propulsion.thrusters.min", -100); - declare_parameter("propulsion.thrusters.max", 100); - declare_parameter("propulsion.thrusters.direction", std::vector{0}); - declare_parameter("propulsion.thrusters.configuration_matrix", - std::vector{0}); - - num_dof_ = get_parameter("propulsion.dofs.num").as_int(); - num_thrusters_ = get_parameter("propulsion.thrusters.num").as_int(); - min_thrust_ = get_parameter("propulsion.thrusters.min").as_int(); - max_thrust_ = get_parameter("propulsion.thrusters.max").as_int(); - direction_ = - get_parameter("propulsion.thrusters.direction").as_integer_array(); - - thrust_configuration = doubleArrayToEigenMatrix( - get_parameter("propulsion.thrusters.configuration_matrix") - .as_double_array(), - num_dof_, num_thrusters_); - - wrench_subscriber_ = this->create_subscription( - "thrust/wrench_input", 1, - std::bind(&ThrusterAllocator::wrench_callback, this, - std::placeholders::_1)); - - thrust_publisher_ = this->create_publisher( - "thrust/thruster_forces", 1); - - timer_ = this->create_wall_timer( - 100ms, std::bind(&ThrusterAllocator::timer_callback, this)); - - pseudoinverse_allocator_.T_pinv = - calculateRightPseudoinverse(thrust_configuration); - - body_frame_forces_.setZero(); -} - -void ThrusterAllocator::timer_callback() { - Eigen::VectorXd thruster_forces = - pseudoinverse_allocator_.calculateAllocatedThrust(body_frame_forces_); - - if (isInvalidMatrix(thruster_forces)) { - RCLCPP_ERROR(get_logger(), "ThrusterForces vector invalid"); - return; - } - - if (!saturateVectorValues(thruster_forces, min_thrust_, max_thrust_)) { - RCLCPP_WARN(get_logger(), "Thruster forces vector required saturation."); - } - - std_msgs::msg::Float32MultiArray msg_out; - arrayEigenToMsg(thruster_forces, msg_out); - thrust_publisher_->publish(msg_out); -} - -void ThrusterAllocator::wrench_callback(const geometry_msgs::msg::Wrench &msg) { - Eigen::Vector3d msg_vector; - msg_vector(0) = msg.force.x; // surge - msg_vector(1) = msg.force.y; // sway - msg_vector(2) = msg.torque.z; // yaw - if (!healthyWrench(msg_vector)) { - RCLCPP_ERROR(get_logger(), "ASV wrench vector invalid, ignoring."); - return; - } - std::swap(msg_vector, body_frame_forces_); -} - -bool ThrusterAllocator::healthyWrench(const Eigen::VectorXd &v) const { - if (isInvalidMatrix(v)) - return false; - - bool within_max_thrust = std::none_of(v.begin(), v.end(), [this](double val) { - return std::abs(val) > max_thrust_; - }); - - return within_max_thrust; -} diff --git a/motion/thruster_allocator/src/pseudoinverse_allocator.cpp b/motion/thruster_allocator/src/pseudoinverse_allocator.cpp deleted file mode 100644 index 8b553763..00000000 --- a/motion/thruster_allocator/src/pseudoinverse_allocator.cpp +++ /dev/null @@ -1,10 +0,0 @@ -#include "thruster_allocator/pseudoinverse_allocator.hpp" - -PseudoinverseAllocator::PseudoinverseAllocator(const Eigen::MatrixXd &T_pinv) - : T_pinv(T_pinv) {} - -Eigen::VectorXd -PseudoinverseAllocator::calculateAllocatedThrust(const Eigen::VectorXd &tau) { - Eigen::VectorXd u = T_pinv * tau; - return u; -} \ No newline at end of file diff --git a/motion/thruster_allocator/src/thruster_allocator_node.cpp b/motion/thruster_allocator/src/thruster_allocator_node.cpp deleted file mode 100644 index 77c604b7..00000000 --- a/motion/thruster_allocator/src/thruster_allocator_node.cpp +++ /dev/null @@ -1,11 +0,0 @@ -#include "thruster_allocator/allocator_ros.hpp" -#include "thruster_allocator/allocator_utils.hpp" - -int main(int argc, char **argv) { - rclcpp::init(argc, argv); - auto allocator = std::make_shared(); - RCLCPP_INFO(allocator->get_logger(), "Thruster allocator initiated"); - rclcpp::spin(allocator); - rclcpp::shutdown(); - return 0; -} diff --git a/motion/thruster_interface_asv/CMakeLists.txt b/motion/thruster_interface_asv/CMakeLists.txt deleted file mode 100644 index 5bff0ffc..00000000 --- a/motion/thruster_interface_asv/CMakeLists.txt +++ /dev/null @@ -1,42 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(thruster_interface_asv) - -# Default to C++14 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) -endif() - -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 REQUIRED) -find_package(rclcpp REQUIRED) -find_package(std_msgs REQUIRED) - -include_directories(include) - -add_executable(${PROJECT_NAME}_node - src/thruster_interface_asv_driver_lib.cpp - src/thruster_interface_asv_node.cpp -) - -ament_target_dependencies(${PROJECT_NAME}_node - rclcpp - std_msgs -) - -# Install launch files. -install(DIRECTORY - launch - config - DESTINATION share/${PROJECT_NAME} -) - -install(TARGETS - ${PROJECT_NAME}_node - DESTINATION lib/${PROJECT_NAME} -) - -ament_package() diff --git a/motion/thruster_interface_asv/config/ThrustMe_P1000_force_mapping.csv b/motion/thruster_interface_asv/config/ThrustMe_P1000_force_mapping.csv deleted file mode 100644 index bac8cf21..00000000 --- a/motion/thruster_interface_asv/config/ThrustMe_P1000_force_mapping.csv +++ /dev/null @@ -1,190 +0,0 @@ -"Force(g)","PWM(micros)" --12989.13 1100 --12792.53 1104.49 --12779.61 1108.99 --12635.04 1113.48 --12547.48 1117.98 --12554.38 1122.47 --12528.53 1126.97 --12484.77 1131.46 --12327.03 1135.96 --12337.55 1140.45 --12202.25 1144.94 --12147.19 1149.44 --12097.76 1153.93 --11947.61 1158.43 --11948.59 1162.92 --11878.31 1167.42 --11685.45 1171.91 --11630.87 1176.4 --11660.74 1180.9 --11514.6 1185.39 --11370.6 1189.89 --11293.55 1194.38 --11221.2 1198.88 --11213.27 1203.37 --11064.32 1207.87 --11049.23 1212.36 --10945.55 1216.85 --10837.99 1221.35 --10678.96 1225.84 --10754.31 1230.34 --10588.09 1234.83 --10503.28 1239.33 --10344.9 1243.82 --10226.64 1248.31 --10190.24 1252.81 --10081.85 1257.3 --9959.01 1261.8 --9948.11 1266.29 --9759.36 1270.79 --9753.3 1275.28 --9619.75 1279.78 --9525.81 1284.27 --9362.72 1288.76 --9333.51 1293.26 --9187.35 1297.75 --9078.53 1302.25 --8851.82 1306.74 --8732.14 1311.24 --8632.45 1315.73 --8490.62 1320.22 --8411.14 1324.72 --8312.52 1329.21 --8047.98 1333.71 --7948.19 1338.2 --7934.35 1342.7 --7780.13 1347.19 --7709.89 1351.69 --7375.4 1356.18 --7364.05 1360.67 --7233.1 1365.17 --6972.58 1369.66 --6845.09 1374.16 --6759.8 1378.65 --6633.6 1383.15 --6337.56 1387.64 --6224.88 1392.13 --6132.13 1396.63 --5939.88 1401.12 --5682.91 1405.62 --5527.56 1410.11 --5258.96 1414.61 --5074.12 1419.1 --4823.63 1423.6 --4685.91 1428.09 --4500.56 1432.58 --4308.91 1437.08 --4012.16 1441.57 --3866.78 1446.07 --3608.86 1450.56 --3235.68 1455.06 --2964.98 1459.55 --2782.91 1464.04 --2522.36 1468.54 --2112.45 1473.03 --1955.09 1477.53 --1551.28 1482.02 --1327.8 1486.52 --913.3 1491.01 --506.29 1495.51 -0 1500 -614.46 1504.04 -967.25 1508.08 -1501.92 1512.12 -1965.98 1516.16 -2315.18 1520.2 -2789.34 1524.24 -3218.91 1528.28 -3442.74 1532.32 -3763.95 1536.36 -4118.76 1540.4 -4473.32 1544.44 -4733.06 1548.48 -5052.11 1552.53 -5306.67 1556.57 -5514.31 1560.61 -5789.63 1564.65 -6061.47 1568.69 -6337.72 1572.73 -6516.65 1576.77 -6718.8 1580.81 -6901.18 1584.85 -7112.16 1588.89 -7352 1592.93 -7634.54 1596.97 -7720.88 1601.01 -7973.84 1605.05 -8136.83 1609.09 -8386.24 1613.13 -8569.13 1617.17 -8745.66 1621.21 -8921.09 1625.25 -9081.5 1629.29 -9306.64 1633.33 -9397.61 1637.37 -9521.35 1641.41 -9746.14 1645.45 -9890.84 1649.49 -9952.96 1653.54 -10218.41 1657.58 -10275.6 1661.62 -10446.95 1665.66 -10584.48 1669.7 -10754.31 1673.74 -10864.61 1677.78 -10959.78 1681.82 -11109.23 1685.86 -11210.52 1689.9 -11367.21 1693.94 -11528.98 1697.98 -11604.96 1702.02 -11700.44 1706.06 -11756.9 1710.1 -11942.16 1714.14 -12045.06 1718.18 -12179.5 1722.22 -12383.12 1726.26 -12317.95 1730.3 -12531.05 1734.34 -12581.61 1738.38 -12716.21 1742.42 -12825.03 1746.46 -12832.83 1750.51 -13053.31 1754.55 -13181.88 1758.59 -13207.91 1762.63 -13247.19 1766.67 -13414.53 1770.71 -13466.28 1774.75 -13573.65 1778.79 -13696.17 1782.83 -13803.27 1786.87 -13847.3 1790.91 -14029.66 1794.95 -14067.08 1798.99 -14156.74 1803.03 -14283.08 1807.07 -14300.44 1811.11 -14443.34 1815.15 -14541.8 1819.19 -14560.88 1823.23 -14584.52 1827.27 -14665.63 1831.31 -14718.97 1835.35 -14901.01 1839.39 -14973.02 1843.43 -15054.56 1847.47 -15149.7 1851.52 -15239.98 1855.56 -15272.75 1859.6 -15281.37 1863.64 -15384.03 1867.68 -15419.57 1871.72 -15593.75 1875.76 -15602.55 1879.8 -15724.38 1883.84 -15696.74 1887.88 -15764.41 1891.92 -15910.14 1895.96 -15958.35 1900 \ No newline at end of file diff --git a/motion/thruster_interface_asv/include/thruster_interface_asv/thruster_interface_asv_driver_lib.hpp b/motion/thruster_interface_asv/include/thruster_interface_asv/thruster_interface_asv_driver_lib.hpp deleted file mode 100644 index a306b75d..00000000 --- a/motion/thruster_interface_asv/include/thruster_interface_asv/thruster_interface_asv_driver_lib.hpp +++ /dev/null @@ -1,20 +0,0 @@ -// C++ Libraries -#include -#include -#include -#include -#include -#include -#include - -#include // Used for the O_RDWR define -#include // Used for the I2C_SLAVE define -#include -#include // Used for the close function - -namespace thruster_interface_asv_driver_lib { -void init(const std::string &pathToCSVFile, int8_t *thrusterMapping, - int8_t *thrusterDirection, int16_t *offsetPWM, int16_t *minPWM, - int16_t *maxPWM); -int16_t *drive_thrusters(float *thrusterForces); -} // namespace thruster_interface_asv_driver_lib \ No newline at end of file diff --git a/motion/thruster_interface_asv/include/thruster_interface_asv/thruster_interface_asv_node.hpp b/motion/thruster_interface_asv/include/thruster_interface_asv/thruster_interface_asv_node.hpp deleted file mode 100644 index f2ebfcdb..00000000 --- a/motion/thruster_interface_asv/include/thruster_interface_asv/thruster_interface_asv_node.hpp +++ /dev/null @@ -1,12 +0,0 @@ -// C++ Libraries -#include - -// ROS2 libraries -#include -#include // For the ROS2 Timer clock -#include -#include -#include - -// Custom libraries -#include diff --git a/motion/thruster_interface_asv/launch/thruster_interface_asv.launch.py b/motion/thruster_interface_asv/launch/thruster_interface_asv.launch.py deleted file mode 100755 index 8ddeb502..00000000 --- a/motion/thruster_interface_asv/launch/thruster_interface_asv.launch.py +++ /dev/null @@ -1,19 +0,0 @@ -from launch import LaunchDescription -from launch_ros.actions import Node -from ament_index_python.packages import get_package_share_directory -from os import path - -def generate_launch_description(): - # Path to the YAML file - config = path.join(get_package_share_directory("asv_setup"),'config', 'robots', "freya.yaml") - - thruste_interface_asv_node = Node( - package='thruster_interface_asv', - namespace='thruster_interface_asv', - executable='thruster_interface_asv_node', - name='thruster_interface_asv_node', - output='screen', - parameters=[config], - ) - - return LaunchDescription([thruste_interface_asv_node]) \ No newline at end of file diff --git a/motion/thruster_interface_asv/package.xml b/motion/thruster_interface_asv/package.xml deleted file mode 100644 index 8672c824..00000000 --- a/motion/thruster_interface_asv/package.xml +++ /dev/null @@ -1,25 +0,0 @@ - - - - thruster_interface_asv - 0.0.0 - Interface for transforming thrust to thruster rpm - alekskl01 - MIT - - ament_cmake - - rclcpp - std_msgs - ament_index_cpp - - ament_lint_auto - ament_lint_common - ament_cmake_gtest - - ros2launch - - - ament_cmake - - diff --git a/motion/thruster_interface_asv/src/thruster_interface_asv_driver_lib.cpp b/motion/thruster_interface_asv/src/thruster_interface_asv_driver_lib.cpp deleted file mode 100644 index 24799953..00000000 --- a/motion/thruster_interface_asv/src/thruster_interface_asv_driver_lib.cpp +++ /dev/null @@ -1,234 +0,0 @@ -#include - -namespace thruster_interface_asv_driver_lib { -// Interpolation from forces to pwm functions (START) ---------- -struct _ForcePWM { - float force; - float pwm; -}; - -// Local variables used inside the interpolation -std::vector<_ForcePWM> _ForcePWMTable; - -std::vector<_ForcePWM> _loadDataFromCSV(const std::string &filepath) { - // Prepare the datastructure we will load our data in - std::vector<_ForcePWM> data; - - // Try opening the file and if suceeded fill our datastructure with all the - // values from the force mapping .CSV file - std::ifstream file(filepath); - if (file.is_open()) { - std::string line; - - // Skip the header line - std::getline(file, line); - - // Read data line by line - while (std::getline(file, line)) { - // Temporarty variables to store data correctly ---------- - // Define temporary placeholders for variables we are extracting - std::string tempVar; - - // Define data structure format we want our .CSV vlaues to be ordered in - _ForcePWM ForcePWMDataStructure; - - // Data manipulation ---------- - // csvLineSplit variable converts "line" variable to a char stream of data - // that can further be used to split and filter out values we want - std::istringstream csvLineSplit(line); - - // Extract Forces from "csvLineSplit" variable - std::getline(csvLineSplit, tempVar, '\t'); - ForcePWMDataStructure.force = std::stof(tempVar); - - // Convert grams into Newtons as we expect to get Forces in Newtons but - // the .CSV file contains forces in grams - ForcePWMDataStructure.force = - ForcePWMDataStructure.force * (9.81 / 1000.0); - - // Extract PWM from "csvLineSplit" variable - std::getline(csvLineSplit, tempVar, '\t'); - ForcePWMDataStructure.pwm = std::stof(tempVar); - - // Push processed data with correct formating into the complete dataset - // ---------- - data.push_back(ForcePWMDataStructure); - } - - file.close(); - } else { - std::cerr << "ERROR: Unable to open file: " << filepath << std::endl; - exit(1); - } - - return data; -} - -int16_t *_interpolate_force_to_pwm(float *forces) { - int16_t *interpolatedPWMArray = new int16_t[4]{0, 0, 0, 0}; - - // Interpolate ---------- - for (int8_t i = 0; i < 4; i++) { - // Edge case, if the force is out of the bounds of your data table, handle - // accordingly - if (forces[i] <= _ForcePWMTable.front().force) { - interpolatedPWMArray[i] = - static_cast(_ForcePWMTable.front().pwm); // Too small Force - - } else if (forces[i] >= _ForcePWMTable.back().force) { - interpolatedPWMArray[i] = - static_cast(_ForcePWMTable.back().pwm); // Too big Force - - } else { - // Set temporary variables for interpolating - // Initialize with the first element - _ForcePWM low = _ForcePWMTable.front(); - _ForcePWM high; - - // Interpolate - // Find the two points surrounding the given force - // Run the loop until the force value we are givven is lower than the - // current dataset value - for (const _ForcePWM &CurrentForcePWMData : _ForcePWMTable) { - if (CurrentForcePWMData.force >= forces[i]) { - high = CurrentForcePWMData; - break; - } - low = CurrentForcePWMData; - } - - // Linear interpolation formula: y = y0 + (y1 - y0) * ((x - x0) / (x1 - - // x0)) - float interpolatedPWM = - low.pwm + (high.pwm - low.pwm) * - ((forces[i] - low.force) / (high.force - low.force)); - - // Save the interpolated value in the final array - interpolatedPWMArray[i] = static_cast(interpolatedPWM); - } - } - - return interpolatedPWMArray; -} -// Interpolation from forces to pwm functions (STOP) ---------- - -// The most important function that sends PWM values through I2C to Arduino that -// then sends PWM signal further to the ESCs -const int8_t _I2C_BUS = 1; -const int16_t _I2C_ADDRESS = 0x21; -const char *_I2C_DEVICE = "/dev/i2c-1"; -int _fileI2C = -1; -void _send_pwm_to_ESCs(int16_t *pwm) { - // Variables ---------- - // 4 PWM values of 16-bits - // Each byte is 8-bits - // 4*16-bits = 64-bits => 8 bytes - uint8_t dataSize = 8; - uint8_t messageInBytesPWM[8]; - - // Convert PWM int 16-bit to bytes ---------- - for (int8_t i = 0; i < 4; i++) { - // Split the integer into most significant byte (MSB) and least significant - // byte (LSB) - uint8_t msb = (pwm[i] >> 8) & 0xFF; - uint8_t lsb = pwm[i] & 0xFF; - - // Add the bytes to the finished array - messageInBytesPWM[i * 2] = msb; - messageInBytesPWM[i * 2 + 1] = lsb; - } - - // Data sending ---------- - try { - // Send the I2C message - if (write(_fileI2C, messageInBytesPWM, dataSize) != dataSize) { - throw std::runtime_error( - "ERROR: Couldn't send data, ignoring message..."); - } - } catch (const std::exception &error) { - std::cerr << error.what() << std::endl; - } -} - -// Initial function to set everything up with thruster driver -// Set initial PWM limiting variables -int8_t _thrusterMapping[4]; -int8_t _thrusterDirection[4]; -int16_t _offsetPWM[4]; -int16_t _minPWM[4]; -int16_t _maxPWM[4]; - -void init(const std::string &pathToCSVFile, int8_t *thrusterMapping, - int8_t *thrusterDirection, int16_t *offsetPWM, int16_t *minPWM, - int16_t *maxPWM) { - // Load data from the .CSV file - _ForcePWMTable = _loadDataFromCSV(pathToCSVFile); - - // Set correct parameters - for (int8_t i = 0; i < 4; i++) { - _thrusterMapping[i] = thrusterMapping[i]; - _thrusterDirection[i] = thrusterDirection[i]; - _offsetPWM[i] = offsetPWM[i]; - _minPWM[i] = minPWM[i]; - _maxPWM[i] = maxPWM[i]; - } - - // Connecting to the I2C - try { - // Open I2C conection - _fileI2C = open(_I2C_DEVICE, O_RDWR); - - // Error handling in case of edge cases with I2C - if (_fileI2C < 0) { - throw std::runtime_error("ERROR: Couldn't opening I2C device"); - } - if (ioctl(_fileI2C, I2C_SLAVE, _I2C_ADDRESS) < 0) { - throw std::runtime_error("ERROR: Couldn't set I2C address"); - } - } catch (const std::exception &error) { - std::cerr << error.what() << std::endl; - } -} - -// The main core functionality of interacting and controling the thrusters -int16_t *drive_thrusters(float *thrusterForces) { - - // Change direction of the thruster (Forward(+1)/Backwards(-1)) according to - // the direction parameter - float thrusterForcesChangedDirection[4] = {0.0, 0.0, 0.0, 0.0}; - for (int8_t i = 0; i < 4; i++) { - thrusterForcesChangedDirection[i] = - thrusterForces[i] * _thrusterDirection[i]; - } - - // Remap Thrusters - // From: [pin1:thruster1, pin2:thruster2, pin3:thruster3, pin4:thruster4] - // To: [pin1:, pin2:, - // pin3:, pin4:] - float thrusterForcesChangedMapping[4] = {0.0, 0.0, 0.0, 0.0}; - for (int8_t pinNr = 0; pinNr < 4; pinNr++) { - int8_t remapedThrusterForcesIndex = _thrusterMapping[pinNr]; - thrusterForcesChangedMapping[pinNr] = - thrusterForcesChangedDirection[remapedThrusterForcesIndex]; - } - - // Interpolate forces to raw PWM values - int16_t *pwm = _interpolate_force_to_pwm(thrusterForcesChangedMapping); - - // Offset PWM - for (int8_t i = 0; i < 4; i++) { - pwm[i] += _offsetPWM[i]; - } - - // Limit PWM - for (int8_t i = 0; i < 4; i++) { - pwm[i] = std::clamp(pwm[i], _minPWM[i], _maxPWM[i]); - } - - // Send PWM vlaues through I2C to the microcontroller to control thrusters - _send_pwm_to_ESCs(pwm); - - // Return PWM values for debuging and logging purposes - return pwm; -} -} // namespace thruster_interface_asv_driver_lib diff --git a/motion/thruster_interface_asv/src/thruster_interface_asv_node.cpp b/motion/thruster_interface_asv/src/thruster_interface_asv_node.cpp deleted file mode 100644 index a4ebf83c..00000000 --- a/motion/thruster_interface_asv/src/thruster_interface_asv_node.cpp +++ /dev/null @@ -1,137 +0,0 @@ -// Custom libraies -#include - -class ThrusterInterfaceASVNode : public rclcpp::Node { -private: - // ROS2 Variables ---------- - rclcpp::Subscription::SharedPtr - _subscriberThrusterForces; - rclcpp::Publisher::SharedPtr _publisherPWM; - rclcpp::TimerBase::SharedPtr _thruster_driver_timer; - - // Variables that are shared inside the object ---------- - // Mutable allows modification in const methods - mutable float _thrusterForces[4]; - mutable int16_t *_pwm; - - // Methods ---------- - void _thruster_forces_callback( - const std_msgs::msg::Float32MultiArray::SharedPtr msg) const { - // Save subscribed topic values into universal array for further usage - for (size_t i = 0; i < 4; i++) { - _thrusterForces[i] = msg->data[i]; - } - } - - void _thruster_driver_callback() { - // Drive thrusters according to the thrusterForces provided by subscriber - _pwm = thruster_interface_asv_driver_lib::drive_thrusters(_thrusterForces); - - // Publish PWM values for debuging purposes - std::shared_ptr messagePWM = - std::make_shared(); - - messagePWM->data.resize(4); - for (int8_t i = 0; i < 4; i++) { - messagePWM->data[i] = _pwm[i]; - } - _publisherPWM->publish(*messagePWM); - } - -public: - // Builder for the object ---------- - ThrusterInterfaceASVNode() : Node("thruster_interface_asv_node") { - // Thruster Driver Setup ---------- - // Get filepath of .CSV file with force mapping - std::string forcesToPWMDataFilePath = - ament_index_cpp::get_package_share_directory("thruster_interface_asv"); - forcesToPWMDataFilePath += "/config/ThrustMe_P1000_force_mapping.csv"; - - // Get Thruster mapping values from ROS2 Parameters - std::vector parameterThrusterMapping = - this->declare_parameter>( - "propulsion.thrusters.thruster_to_pin_mapping", {0, 1, 2, 3}); - - int8_t ThrusterMapping[4] = { - static_cast(parameterThrusterMapping[0]), - static_cast(parameterThrusterMapping[1]), - static_cast(parameterThrusterMapping[2]), - static_cast(parameterThrusterMapping[3])}; - - // Get Thruster direction values from ROS2 Parameters - std::vector parameterThrusterDirection = - this->declare_parameter>( - "propulsion.thrusters.thruster_direction", {1, 1, 1, 1}); - - int8_t thrusterDirection[4] = { - static_cast(parameterThrusterDirection[0]), - static_cast(parameterThrusterDirection[1]), - static_cast(parameterThrusterDirection[2]), - static_cast(parameterThrusterDirection[3])}; - - // Get PWM Offset values from ROS2 Parameters - std::vector parameterPWMOffset = - this->declare_parameter>( - "propulsion.thrusters.thruster_PWM_offset", {0, 0, 0, 0}); - - int16_t offsetPWM[4] = {static_cast(parameterPWMOffset[0]), - static_cast(parameterPWMOffset[1]), - static_cast(parameterPWMOffset[2]), - static_cast(parameterPWMOffset[3])}; - - // Get MAX and MIN PWM values from ROS2 Parameters - std::vector parameterPWMMin = - this->declare_parameter>( - "propulsion.thrusters.thruster_PWM_min", {1100, 1100, 1100, 1100}); - - std::vector parameterPWMMax = - this->declare_parameter>( - "propulsion.thrusters.thruster_PWM_max", {1900, 1900, 1900, 1900}); - - int16_t minPWM[4] = {static_cast(parameterPWMMin[0]), - static_cast(parameterPWMMin[1]), - static_cast(parameterPWMMin[2]), - static_cast(parameterPWMMin[3])}; - - int16_t maxPWM[4] = {static_cast(parameterPWMMax[0]), - static_cast(parameterPWMMax[1]), - static_cast(parameterPWMMax[2]), - static_cast(parameterPWMMax[3])}; - - // Initialize Thruster driver - thruster_interface_asv_driver_lib::init(forcesToPWMDataFilePath, - ThrusterMapping, thrusterDirection, - offsetPWM, minPWM, maxPWM); - - // ROS Setup ---------- - // Initialize ROS2 thrusterForces subscriber - _subscriberThrusterForces = - this->create_subscription( - "/thrust/thruster_forces", 5, - std::bind(&ThrusterInterfaceASVNode::_thruster_forces_callback, - this, std::placeholders::_1)); - - // Initialize ROS2 pwm publisher - _publisherPWM = - this->create_publisher("/pwm", 5); - - // Initialize a never ending cycle that continuously publishes and drives - // thrusters depending on what the ThrusterForces value is set to from - // ThrusterForces subscriber - using namespace std::chrono_literals; - _thruster_driver_timer = this->create_wall_timer( - 200ms, - std::bind(&ThrusterInterfaceASVNode::_thruster_driver_callback, this)); - - // Debugging ---------- - RCLCPP_INFO(this->get_logger(), - "Initialized thruster_interface_asv_node node"); - } -}; - -int main(int argc, char *argv[]) { - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/motion/vessel_hybridpath_simulator/launch/vessel_hybridpath_simulator.launch.py b/motion/vessel_hybridpath_simulator/launch/vessel_hybridpath_simulator.launch.py deleted file mode 100644 index 5fb51358..00000000 --- a/motion/vessel_hybridpath_simulator/launch/vessel_hybridpath_simulator.launch.py +++ /dev/null @@ -1,15 +0,0 @@ -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(): - vessel_hybridpath_simulator_node = Node( - package='vessel_hybridpath_simulator', - executable='vessel_hybridpath_simulator_node', - name='vessel_hybridpath_simulator_node', - output='screen', - ) - return LaunchDescription([ - vessel_hybridpath_simulator_node - ]) diff --git a/motion/vessel_hybridpath_simulator/package.xml b/motion/vessel_hybridpath_simulator/package.xml deleted file mode 100644 index d684d04c..00000000 --- a/motion/vessel_hybridpath_simulator/package.xml +++ /dev/null @@ -1,24 +0,0 @@ - - - - vessel_hybridpath_simulator - 0.0.0 - TODO: Package description - vortex - MIT - - rclpy - nav_msgs - python-transforms3d-pip - geometry_msgs - vortex_msgs - - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - - - ament_python - - diff --git a/motion/vessel_hybridpath_simulator/resource/vessel_hybridpath_simulator b/motion/vessel_hybridpath_simulator/resource/vessel_hybridpath_simulator deleted file mode 100644 index e69de29b..00000000 diff --git a/motion/vessel_hybridpath_simulator/setup.cfg b/motion/vessel_hybridpath_simulator/setup.cfg deleted file mode 100644 index a931c412..00000000 --- a/motion/vessel_hybridpath_simulator/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/vessel_hybridpath_simulator -[install] -install_scripts=$base/lib/vessel_hybridpath_simulator diff --git a/motion/vessel_hybridpath_simulator/setup.py b/motion/vessel_hybridpath_simulator/setup.py deleted file mode 100644 index a8add765..00000000 --- a/motion/vessel_hybridpath_simulator/setup.py +++ /dev/null @@ -1,29 +0,0 @@ -import os -from glob import glob -from setuptools import find_packages, setup - -package_name = 'vessel_hybridpath_simulator' - -setup( - name=package_name, - version='0.0.0', - packages=find_packages(exclude=['test']), - data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), - (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), - ], - install_requires=['setuptools'], - zip_safe=True, - maintainer='martin', - maintainer_email='martiniuss.wan@gmail.com', - description='TODO: Package description', - license='TODO: License declaration', - tests_require=['pytest'], - entry_points={ - 'console_scripts': [ - 'vessel_hybridpath_simulator_node = vessel_hybridpath_simulator.vessel_hybridpath_simulator_node:main' - ], - }, -) diff --git a/motion/vessel_hybridpath_simulator/vessel_hybridpath_simulator/__init__.py b/motion/vessel_hybridpath_simulator/vessel_hybridpath_simulator/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/motion/vessel_hybridpath_simulator/vessel_hybridpath_simulator/vessel_hybridpath_simulator_node.py b/motion/vessel_hybridpath_simulator/vessel_hybridpath_simulator/vessel_hybridpath_simulator_node.py deleted file mode 100644 index 45805380..00000000 --- a/motion/vessel_hybridpath_simulator/vessel_hybridpath_simulator/vessel_hybridpath_simulator_node.py +++ /dev/null @@ -1,195 +0,0 @@ -import rclpy -import numpy as np -import time - -from rclpy.node import Node -import matplotlib.pyplot as plt -from nav_msgs.msg import Odometry -from geometry_msgs.msg import Wrench -from vortex_msgs.msg import HybridpathReference -from hybridpath_controller.adaptive_backstep import AdaptiveBackstep -from hybridpath_guidance.hp_guidance_utils import Rot -from transforms3d.euler import euler2quat - - -class VesselHybridpathSimulatorNode(Node): - def __init__(self): - super().__init__("vessel_hybridpath_simulator_node") - - self.wrench_subscriber_ = self.create_subscription(Wrench, "thrust/wrench_input", self.wrench_input_cb, 15) - self.hp_ref_subscriber_ = self.create_subscription(HybridpathReference, "guidance/hybridpath/reference", self.hp_ref_cb, 20) - self.state_publisher_ = self.create_publisher(Odometry, "/sensor/seapath/odometry/ned", 1) - - self.T = 200.0 - self.dt = 0.05 - - self.time = np.arange(0, self.T, self.dt) - - self.i = 1 # Step counter - self.i_ref = 1 # Step counter for guidance ref - - waypoints = np.array([[10, 0], - [10, 10], - [0, 20], - [30, 30], - [40,0], - [0,-10], - [0, 0], - [10, 0]]) - - - self.eta_d = np.zeros((3, len(self.time))) - self.eta = np.zeros((3, len(self.time))) - self.eta_d[:,0] = np.array([waypoints[0,0], waypoints[0,1], np.pi/2]) - self.eta[:,0] = np.array([waypoints[0,0], waypoints[0,1], np.pi/2]) - self.nu = np.zeros((3, len(self.time))) - self.tau = np.zeros((3, len(self.time))) - - self.AB = AdaptiveBackstep() - - time.sleep(1) - odometry_msg = self.state_to_odometrymsg() - self.state_publisher_.publish(odometry_msg) - - - self.get_logger().info("vessel_hybridpath_simulator_node started") - self.get_logger().info("""\ - - ._ o o - \_`-)|_ - ,"" \ - ," ## | ಠ ಠ. - ," ## ,-\__ `. - ," / `--._;) - ," ## / U - ," ## / - - Simulation time: """ + str(self.T) + """ secs. Please wait for sim to finish:) - """) - - def wrench_input_cb(self, msg: Wrench): - if self.i == len(self.time): - self.plot_simulation() - return - if self.i_ref > len(self.time): - return - - self.tau[:, self.i] = np.array([msg.force.x, msg.force.y, msg.torque.z]) - - # Step in nu and eta - nu_dot = np.linalg.inv(self.AB.M) @ self.tau[:, self.i] - np.linalg.inv(self.AB.M) @ self.AB.D @ self.nu[:, self.i-1] - self.nu[:, self.i] = self.nu[:, self.i-1] + nu_dot * self.dt - R, R_trsp = Rot(self.eta[2,self.i-1]) - eta_dot = R @ self.nu[:, self.i] - self.eta[:, self.i] = self.eta[:, self.i-1] + eta_dot * self.dt - - # SSA - psi0 = self.eta[2, self.i-1] - psi1 = self.eta[2, self.i] - psi_vec = np.array([psi0, psi1]) - psi_vec = np.unwrap(psi_vec, period=2*np.pi) - self.eta[2, self.i] = psi_vec[1] - - # Publish state - odometry_msg = self.state_to_odometrymsg() - self.state_publisher_.publish(odometry_msg) - - self.i += 1 - - - def hp_ref_cb(self, msg: HybridpathReference): - if self.i_ref < self.eta_d.shape[1]: - self.eta_d[:, self.i_ref] = np.array([msg.eta_d.x, msg.eta_d.y, msg.eta_d.theta]) - else: - self.get_logger().error('Index out of bounds: self.i_ref is too large') - self.i_ref += 1 - - def state_to_odometrymsg(self): - orientation_list_next = euler2quat(0, 0, self.eta[2, self.i]) - - odometry_msg = Odometry() - odometry_msg.pose.pose.position.x = self.eta[0, self.i] - odometry_msg.pose.pose.position.y = self.eta[1, self.i] - 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] - odometry_msg.twist.twist.linear.x = self.nu[0, self.i] - odometry_msg.twist.twist.linear.y = self.nu[1, self.i] - odometry_msg.twist.twist.linear.z = 0.0 - odometry_msg.twist.twist.angular.x = 0.0 - odometry_msg.twist.twist.angular.y = 0.0 - odometry_msg.twist.twist.angular.z = self.nu[2, self.i] - - return odometry_msg - - - def plot_simulation(self): - # np.savetxt('/home/eta_d_ros.txt', self.eta_d.T) - # Plotting - plt.figure() - plt.plot(self.eta_d[1,:], self.eta_d[0,:], label='Reference path', zorder = 0) - plt.plot(self.eta[1,:], self.eta[0,:], label='Actual path', zorder = 1) - for i in range(0, len(self.eta_d[2]), 100): - plt.quiver(self.eta[1,i], self.eta[0,i], np.sin(self.eta[2,i]), np.cos(self.eta[2,i]), zorder = 2) - plt.title('Actual path vs reference path') - plt.xlabel('y [m]') - plt.ylabel('x [m]') - plt.gca().set_axisbelow(True) - plt.grid() - plt.legend() - - plt.figure() - plt.plot(self.time, self.eta[0,:], label='Actual x') - plt.plot(self.time, self.eta_d[0,:], label='Reference x') - plt.plot(self.time, self.eta[1,:], label='Actual y') - plt.plot(self.time, self.eta_d[1,:], 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.plot(self.time, self.eta[2,:], label='Actual heading') - plt.plot(self.time, self.eta_d[2,:], label='Reference heading') - plt.title('Actual heading vs reference heading') - plt.xlabel('Time [s]') - plt.ylabel('Heading [rad]') - plt.gca().set_axisbelow(True) - plt.grid() - plt.legend() - - plt.figure() - plt.plot(self.time, self.nu[0,:], label='Surge velocity') - plt.plot(self.time, self.nu[1,:], label='Sway velocity') - plt.title('velocity') - plt.xlabel('Time [s]') - plt.ylabel('Velocity [m/s]') - plt.gca().set_axisbelow(True) - plt.grid() - plt.legend() - - plt.figure() - plt.plot(self.time, self.tau[0,:], label='Surge force') - plt.plot(self.time, self.tau[1,:], label='Sway force') - plt.title('Force') - plt.xlabel('Time [s]') - plt.ylabel('Force [N]') - plt.gca().set_axisbelow(True) - plt.grid() - plt.legend() - - plt.show() - -def main(args=None): - rclpy.init(args=args) - node = VesselHybridpathSimulatorNode() - rclpy.spin(node) - node.destroy_node() - rclpy.shutdown() - -if __name__ == "__main__": - main() diff --git a/motion/vessel_simulator/LICENSE b/motion/vessel_simulator/LICENSE deleted file mode 100644 index 30e8e2ec..00000000 --- a/motion/vessel_simulator/LICENSE +++ /dev/null @@ -1,17 +0,0 @@ -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in -all copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -THE SOFTWARE. diff --git a/motion/vessel_simulator/launch/vessel_simulator.launch.py b/motion/vessel_simulator/launch/vessel_simulator.launch.py deleted file mode 100644 index e9c7d5cb..00000000 --- a/motion/vessel_simulator/launch/vessel_simulator.launch.py +++ /dev/null @@ -1,17 +0,0 @@ -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(): - vessel_simulator_node = Node( - package='vessel_simulator', - executable='vessel_simulator_node', - name='vessel_simulator_node', - # parameters=[os.path.join(get_package_share_directory('vessel_simulator'),'config','lqr_config.yaml')], - output='screen', - ) - return LaunchDescription([ - vessel_simulator_node - ]) - diff --git a/motion/vessel_simulator/package.xml b/motion/vessel_simulator/package.xml deleted file mode 100644 index 00033eb7..00000000 --- a/motion/vessel_simulator/package.xml +++ /dev/null @@ -1,29 +0,0 @@ - - - - vessel_simulator - 0.0.0 - TODO: Package description - vortex - MIT - - lqr_controller - rclpy - std_msgs - geometry_msgs - nav_msgs - transforms3d - numpy - matplotlib - scipy - - - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - - - ament_python - - diff --git a/motion/vessel_simulator/resource/vessel_simulator b/motion/vessel_simulator/resource/vessel_simulator deleted file mode 100644 index e69de29b..00000000 diff --git a/motion/vessel_simulator/setup.cfg b/motion/vessel_simulator/setup.cfg deleted file mode 100644 index d28c4cc5..00000000 --- a/motion/vessel_simulator/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/vessel_simulator -[install] -install_scripts=$base/lib/vessel_simulator diff --git a/motion/vessel_simulator/setup.py b/motion/vessel_simulator/setup.py deleted file mode 100644 index b45e8486..00000000 --- a/motion/vessel_simulator/setup.py +++ /dev/null @@ -1,30 +0,0 @@ -import os -from glob import glob -from setuptools import find_packages, setup - -package_name = 'vessel_simulator' - -setup( - name=package_name, - version='0.0.0', - packages=find_packages(exclude=['test']), - data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), - (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), - (os.path.join('share', package_name, 'config'), glob('config/*.yaml')), - ], - install_requires=['setuptools'], - zip_safe=True, - maintainer='vortex', - maintainer_email='vortex.git@vortexntnu.no', - description='TODO: Package description', - license='MIT', - tests_require=['pytest'], - entry_points={ - 'console_scripts': [ - 'vessel_simulator_node = vessel_simulator.vessel_simulator_node:main' - ], - }, -) diff --git a/motion/vessel_simulator/vessel_simulator/__init__.py b/motion/vessel_simulator/vessel_simulator/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/motion/vessel_simulator/vessel_simulator/vessel_simulator_node.py b/motion/vessel_simulator/vessel_simulator/vessel_simulator_node.py deleted file mode 100644 index 1595c00f..00000000 --- a/motion/vessel_simulator/vessel_simulator/vessel_simulator_node.py +++ /dev/null @@ -1,217 +0,0 @@ -import rclpy -from rclpy.node import Node - -from geometry_msgs.msg import Wrench -from nav_msgs.msg import Odometry -from transforms3d.euler import euler2quat, quat2euler - -import numpy as np -import matplotlib.pyplot as plt -import time - -def ssa(angle): - return (angle + np.pi) % (2 * np.pi) - np.pi - -class VesselVisualizerNode(Node): - def __init__(self): - super().__init__("vessel_visualizer_node") - # add wrench_input subscriber - self.wrench_subscriber_ = self.create_subscription(Wrench, "thrust/wrench_input", self.wrench_input_cb, 1) - self.guidance_subscriber_ = self.create_subscription(Odometry, "controller/lqr/reference", self.guidance_cb, 1) - - # publish state (seapath) - self.state_publisher_ = self.create_publisher(Odometry, "/sensor/seapath/odometry/ned", 1) - - self.state = np.array([10, -20, 40*np.pi/180, 0, 0, 0]) - self.x_ref = np.array([0, 0, 50*np.pi/180]) # Note to self: make x_ref have 6 states - - self.num_steps_simulated = 0 - - m = 50.0 - self.M = np.diag([m, m, m]) - self.M_inv = np.linalg.inv(self.M) - self.D = np.diag([10.0, 10.0, 5.0]) - - self.T = 200.0 - self.dt = 0.01 - self.num_steps = int(self.T / self.dt) - - self.time = np.linspace(0, self.T, self.num_steps+1) - self.x_history = np.zeros((self.num_steps+1, 6)) #6 rows in A - self.x_ref_history = np.zeros((self.num_steps+1, 3)) # defined as length 3 for now - self.u_history = np.zeros((self.num_steps+1, 3)) #3 columns in B - - self.get_logger().info("vessel_visualizer_node started") - self.get_logger().info("len(self.x_history): " + str(len(self.x_history)) + "self.num_steps" + str(self.num_steps)) - - # Send x_init - self.get_logger().info("Simulation starting in 1 second...") - time.sleep(1) - - self.get_logger().info("""\ - - ._ o o - \_`-)|_ - ,"" \ - ," ## | ಠ ಠ. - ," ## ,-\__ `. - ," / `--._;) - ," ## / U - ," ## / - - Waiting for simulation to finish...""" + str(self.T) + """ secs approximated waiting time :) - """) - self.state_publisher_.publish(self.state_to_odometrymsg(self.state)) - - def odometrymsg_to_state(self, 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 guidance_cb(self, msg): - self.x_ref = self.odometrymsg_to_state(msg)[:3] - - def state_to_odometrymsg(self, 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 wrench_input_cb(self, msg): - u = np.array([msg.force.x, msg.force.y, msg.torque.z]) - x_next = self.RK4_integration_step(self.state, u, self.dt) - self.x_history[self.num_steps_simulated] = x_next - self.x_ref_history[self.num_steps_simulated, : ] = self.x_ref - self.u_history[self.num_steps_simulated] = u - # print(f"self.x_ref_history[{self.num_steps_simulated}]: {self.x_ref_history[self.num_steps_simulated]}") - - if (self.num_steps_simulated >= self.num_steps): - self.plot_history() - return - - odometry_msg = self.state_to_odometrymsg(x_next) - - # Update state - self.state = x_next - - self.state_publisher_.publish(odometry_msg) - self.num_steps_simulated += 1 - - self.get_logger().info(str(self.num_steps_simulated) + " lol xD") # heh DO NOT REMOVE - - def state_dot(self, 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:] = - self.M_inv @ self.D - - B = np.zeros((6,3)) - B[3:,:] = self.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(self, x: np.ndarray, u: np.ndarray, dt: float) -> np.ndarray: - # integration scheme for simulation, implements the Runge-Kutta 4 integrator - k1 = self.state_dot(x, u) - k2 = self.state_dot(x+dt/2*k1, u) - k3 = self.state_dot(x+dt/2*k2, u) - k4 = self.state_dot(x+dt*k3, u) - - x_next = x + dt/6*(k1+2*k2+2*k3+k4) - - return x_next - - def plot_history(self): - # Plot results - # self.get_logger().info(str(self.x_history[:,0])) - # print(self.x_history[:,0]) - # print(self.x_history[:,1]) - - # Save the array to a text file - # file_path = '/home/martin/x_ref_hist.txt' # martin stays - # np.savetxt(file_path, self.x_ref_history, fmt='%4f', delimiter=',') - - plt.figure(figsize=(12, 8)) - plt.subplot(3, 1, 1) - for j in range(3): - plt.plot(self.time, self.x_history[:, j], label=f'State (x_{j+1})') - plt.plot(self.time, self.x_ref_history[:, j], linestyle='--', label=f'Reference (x_ref_{j+1})') - plt.xlabel('Time') - plt.ylabel('State Value') - plt.legend() - - plt.subplot(3, 1, 2) - for j in range(3): #3 columns in B - plt.plot(self.time, self.u_history[:, j], label=f'Control Input (u_{j+1})') - plt.xlabel('Time') - plt.ylabel('Control Input Value') - plt.legend() - - plt.subplot(3, 1, 3) - plt.scatter(self.x_history[:, 0], self.x_history[:, 1], label='Position', s=5) - - p0 = [0.0, 0.0] - p1 = [20.0, 20.0] - p2 = [50.0, -40.0] - p3 = [20.0, -80.0] - p4 = [120.0, -60.0] - p5 = [160, 0.0] - p6 = [60.0, 60.0] - plt.plot([p0[0], p1[0]], [p0[1], p1[1]], 'r-', label='Path') - plt.plot([p1[0], p2[0]], [p1[1], p2[1]], 'g-', label='Path') - plt.plot([p2[0], p3[0]], [p2[1], p3[1]], 'b-', label='Path') - plt.plot([p3[0], p4[0]], [p3[1], p4[1]], 'y-', label='Path') - plt.plot([p4[0], p5[0]], [p4[1], p5[1]], 'm-', label='Path') - plt.plot([p5[0], p6[0]], [p5[1], p6[1]], 'c-', label='Path') - - plt.xlabel('x') - plt.ylabel('y') - plt.legend() - - plt.tight_layout() - plt.show() - -def main(args=None): - rclpy.init(args=args) - node = VesselVisualizerNode() - rclpy.spin(node) - node.destroy_node() - rclpy.shutdown() - -if __name__ == "__main__": - main()