Merge branch 'vessel_sim_ROS2' into LQR_ROS2

asv_setup/launch/ASV_simulator.launch.py

@@ -0,0 +1,35 @@
+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
+    ])

asv_setup/launch/vessel_sim.launch.py

@@ -0,0 +1,35 @@
+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 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 = 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
+    ])

guidance/los_guidance/los_guidance/los_guidance.py

@@ -4,7 +4,7 @@
 class LOSGuidance:
     def __init__(self, p0: list[float], p1: list[float]):
         self.set_path(p0, p1)
-        self.heading_ref = 30*np.pi/180 # magic init number!!!
+        self.heading_ref = 50*np.pi/180 # magic init number!!!
 
     def set_path(self, p0: list[float], p1: list[float]):
         self.p0 = np.array(p0)

guidance/los_guidance/los_guidance/los_guidance_node.py

@@ -14,16 +14,16 @@ def __init__(self):
             parameters=[
                 ('los_guidance.p0', [0.0, 0.0]),
                 ('los_guidance.p1', [0.0, 0.0]),
-                ('los_guidance.look_ahead', 0.0)
+                ('los_guidance.look_ahead_distance', 0.0)
             ])
 
         p0 = self.get_parameter('los_guidance.p0').get_parameter_value().double_array_value
         p1 = self.get_parameter('los_guidance.p1').get_parameter_value().double_array_value
-        self.look_ahead = self.get_parameter('los_guidance.look_ahead').get_parameter_value().double_value
+        self.look_ahead = self.get_parameter('los_guidance.look_ahead_distance').get_parameter_value().double_value
 
         self.get_logger().info(f"p0: {p0}")
         self.get_logger().info(f"p1: {p1}")
-        self.get_logger().info(f"look_ahead: {self.look_ahead}")
+        self.get_logger().info(f"look_ahead_distance: {self.look_ahead}")
 
         self.los_guidance = LOSGuidance(p0, p1)
 
@@ -34,7 +34,7 @@ def __init__(self):
         self.get_logger().info("los_guidance_node started")
 
     def state_cb(self, msg):
-        self.get_logger().info("state_callback")
+        # self.get_logger().info("state_callback")
 
         x = msg.pose.pose.position.x
         y = msg.pose.pose.position.y

motion/lqr_controller/lqr_controller/asv_model.py

@@ -90,15 +90,3 @@ def linearize_model(self, heading: float) -> tuple[np.ndarray, np.ndarray]:
 
         return A, B
 
-    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

motion/lqr_controller/lqr_controller/lqr_controller_node.py

@@ -39,6 +39,7 @@ def __init__(self):
 
         # 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")
 
@@ -63,14 +64,17 @@ def odometrymsg_to_state(self, msg):
     def guidance_cb(self, msg):
         self.x_ref = self.odometrymsg_to_state(msg)[:3]
 
-    def state_cb(self, msg):
-
-        state = self.odometrymsg_to_state(msg)
-
-        self.lqr_controller.linearize_model(state[2])
+        wrench = self.run_lqr_to_wrench()
+
+        # Publish thrust/wrench_input
+        self.wrench_publisher_.publish(wrench)
 
-        # Run LQR 
-        u = self.lqr_controller.calculate_control_input(state, self.x_ref, self.lqr_controller.K_LQR, self.lqr_controller.K_r)
+    def state_cb(self, msg):
+        self.state = self.odometrymsg_to_state(msg)
+
+    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]
@@ -80,8 +84,7 @@ def state_cb(self, msg):
         wrench.torque.y = 0.0
         wrench.torque.z = u[2]
 
-        # Publish thrust/wrench_input
-        self.wrench_publisher_.publish(wrench)
+        return wrench
 
 
 def main(args=None):

motion/vessel_simulator/LICENSE

@@ -0,0 +1,17 @@
+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.

motion/vessel_simulator/launch/vessel_simulator.launch.py

@@ -0,0 +1,17 @@
+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
+    ])
+

motion/vessel_simulator/package.xml

@@ -0,0 +1,29 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>vessel_simulator</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="[email protected]">vortex</maintainer>
+  <license>MIT</license>
+
+  <depend>lqr_controller</depend>
+  <depend>rclpy</depend>
+  <depend>std_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>transforms3d</depend>
+  <depend>numpy</depend>
+  <depend>matplotlib</depend>
+  <depend>scipy</depend>
+
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

motion/vessel_simulator/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/vessel_simulator
+[install]
+install_scripts=$base/lib/vessel_simulator

motion/vessel_simulator/setup.py

@@ -0,0 +1,29 @@
+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')),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='vortex',
+    maintainer_email='[email protected]',
+    description='TODO: Package description',
+    license='MIT',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'vessel_simulator_node = vessel_simulator.vessel_simulator_node:main'
+        ],
+    },
+)