Enhancement/realtime sim

asv_setup/launch/ASV_simulator.launch.py

@@ -0,0 +1,37 @@
+import os
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_xml.launch_description_sources import XMLLaunchDescriptionSource
+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_realtime'), 'launch/vessel_simulator_realtime.launch.py')
+        )
+    )
+
+    print("Make sure Foxglove bridge is installed and running before launching the ASV simulator. See Vortex wiki page for info.")
+
+    # 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/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.

guidance/los_guidance/config/los_guidance_config.yaml

@@ -0,0 +1,6 @@
+/**:
+  ros__parameters:
+    los_guidance:
+      p0: [0.0, 0.0] 
+      p1: [20.0, 20.0]
+      look_ahead_distance: 5.0

guidance/los_guidance/launch/los_guidance.launch.py

@@ -0,0 +1,16 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    los_guidance_node = Node(
+            package='los_guidance',
+            executable='los_guidance_node',
+            name='los_guidance_node',
+            parameters=[os.path.join(get_package_share_directory('los_guidance'),'config','los_guidance_config.yaml')],
+            output='screen',
+        )
+    return LaunchDescription([
+        los_guidance_node
+    ])

guidance/los_guidance/los_guidance/los_guidance.py

@@ -0,0 +1,50 @@
+import numpy as np
+
+class LOSGuidance:
+    def __init__(self, p0: list[float], p1: list[float], p_next: list[float]):
+        self.set_path(p0, p1)
+        self.heading_ref = 50*np.pi/180 # magic init number!!!
+        self.p_next = [np.array([40, 40]), np.array([90.0, -20.0]), np.array([120.0, 50.0])]#, np.array([160, 0.0]), np.array([60.0, 60.0])]
+        self.p_next = [np.array([50, -40]), np.array([20.0, -80.0]), np.array([120.0, -60.0]), np.array([160, 0.0]), np.array([60.0, 60.0])]
+        self.acceptance_radius = 0.5
+
+    def set_path(self, p0: list[float], p1: list[float]):
+        self.p0 = np.array(p0)
+        self.p1 = np.array(p1)
+
+    def calculate_R_Pi_p(self):
+        self.Pi_p = np.arctan2(self.p1[1]-self.p0[1], self.p1[0]-self.p0[0])
+        self.R_Pi_p = np.array(
+            [[np.cos(self.Pi_p), -np.sin(self.Pi_p)],
+            [np.sin(self.Pi_p), np.cos(self.Pi_p)]]
+        )
+
+    def calculate_distance(self, p0, p1):
+        return np.sqrt((p0[0]-p1[0])**2 + (p0[1]-p1[1])**2)
+
+    def switch_path(self):
+        self.p0, self.p1 = self.p1, self.p_next[0]
+
+    def calculate_LOS_x_ref(self, x: np.ndarray, look_ahead: float) -> np.ndarray:
+        self.set_path(self.p0, self.p1)
+        self.calculate_R_Pi_p()
+        p_asv = np.array([x[0], x[1]])
+        errors = self.R_Pi_p.T @ (p_asv - self.p0)
+        along_track_error = errors[0]
+
+        # Switch points to next pair if crossed orthogonal line or entered circle of acceptance
+        if ((along_track_error > 0.8*self.calculate_distance(self.p0, self.p1))):
+            self.switch_path()
+            self.calculate_R_Pi_p()
+            errors = self.R_Pi_p.T @ (p_asv - self.p0)
+            along_track_error = errors[0]
+            self.p_next.pop(0)
+            if (self.p_next == []):
+                self.p_next = [np.array([100000.0, 1000000.0])]
+
+
+        p_los_world = self.R_Pi_p @ np.array([along_track_error + look_ahead, 0]) + self.p0
+        x_ref = np.array([p_los_world[0], p_los_world[1], self.heading_ref])
+
+        return x_ref
+

guidance/los_guidance/los_guidance/los_guidance_node.py

@@ -0,0 +1,87 @@
+import rclpy
+import numpy as np
+from rclpy.node import Node
+from nav_msgs.msg import Odometry
+from transforms3d.euler import quat2euler, euler2quat
+from geometry_msgs.msg import Point
+from los_guidance.los_guidance import LOSGuidance
+
+class LOSGuidanceNode(Node):
+    def __init__(self):
+        super().__init__("los_guidance_node")
+
+        self.declare_parameters(
+            namespace='',
+            parameters=[
+                ('los_guidance.p0', [0.0, 0.0]),
+                ('los_guidance.p1', [0.0, 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_distance').get_parameter_value().double_value
+
+        self.p_next = np.array([10000.0, 10000.0])
+
+        self.get_logger().info(f"p0: {p0}")
+        self.get_logger().info(f"p1: {p1}")
+        self.get_logger().info(f"look_ahead_distance: {self.look_ahead}")
+
+        self.los_guidance = LOSGuidance(p0, p1, self.p_next)
+
+        self.guidance_publisher_ = self.create_publisher(Odometry, "controller/lqr/reference", 1)
+        self.state_subscriber_ = self.create_subscription(Odometry, "/sensor/seapath/odometry/ned", self.state_cb, 1)
+        self.point_subscriber_ = self.create_subscription(Point, "los/point", self.point_cb, 1)
+
+        self.get_logger().info("los_guidance_node started")
+
+    def state_cb(self, msg):
+        # self.get_logger().info("state_callback")
+
+        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)
+
+        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])
+
+        x_ref = self.los_guidance.calculate_LOS_x_ref(state, self.look_ahead)
+
+        orientation_list_ref = euler2quat(roll, pitch, x_ref[2])
+
+        odometry_msg = Odometry()
+        odometry_msg.pose.pose.position.x = x_ref[0]
+        odometry_msg.pose.pose.position.y = x_ref[1]
+        odometry_msg.pose.pose.position.z = 0.0
+        odometry_msg.pose.pose.orientation.w = orientation_list_ref[0]
+        odometry_msg.pose.pose.orientation.x = orientation_list_ref[1]
+        odometry_msg.pose.pose.orientation.y = orientation_list_ref[2]
+        odometry_msg.pose.pose.orientation.z = orientation_list_ref[3]
+
+        self.guidance_publisher_.publish(odometry_msg)
+
+        #rate = rclpy.Rate(100)
+
+    def point_cb(self, msg):
+        self.p_next.append(np.array([msg.x, msg.y])) # TODO: append does not exist for np.arrays
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = LOSGuidanceNode()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == "__main__":
+    main()

guidance/los_guidance/package.xml

@@ -0,0 +1,22 @@
+<?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>los_guidance</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="[email protected]">vortex</maintainer>
+  <license>MIT</license>
+
+  <depend>rclpy</depend>
+  <depend>nav_msgs</depend>
+  <depend>python-transforms3d-pip</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>

guidance/los_guidance/setup.cfg

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

guidance/los_guidance/setup.py

@@ -0,0 +1,30 @@
+import os
+from glob import glob
+from setuptools import find_packages, setup
+
+package_name = 'los_guidance'
+
+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='[email protected]',
+    description='TODO: Package description',
+    license='MIT',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'los_guidance_node = los_guidance.los_guidance_node:main'
+        ],
+    },
+)

motion/lqr_controller/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/lqr_controller/config/lqr_config.yaml

@@ -0,0 +1,7 @@
+/**:
+  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

motion/lqr_controller/launch/lqr_controller.launch.py

@@ -0,0 +1,16 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    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
+    ])