diff --git a/asv_setup/launch/ASV_simulator.launch.py b/asv_setup/launch/ASV_simulator.launch.py new file mode 100644 index 00000000..6205e2cb --- /dev/null +++ b/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, + ]) diff --git a/asv_setup/launch/vessel_sim.launch.py b/asv_setup/launch/vessel_sim.launch.py new file mode 100644 index 00000000..7bea0071 --- /dev/null +++ b/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 + ]) \ No newline at end of file diff --git a/guidance/los_guidance/LICENSE b/guidance/los_guidance/LICENSE new file mode 100644 index 00000000..30e8e2ec --- /dev/null +++ b/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. diff --git a/guidance/los_guidance/config/los_guidance_config.yaml b/guidance/los_guidance/config/los_guidance_config.yaml new file mode 100644 index 00000000..13a0ada6 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/guidance/los_guidance/launch/los_guidance.launch.py b/guidance/los_guidance/launch/los_guidance.launch.py new file mode 100644 index 00000000..f6875f8e --- /dev/null +++ b/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 + ]) diff --git a/guidance/los_guidance/los_guidance/__init__.py b/guidance/los_guidance/los_guidance/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/guidance/los_guidance/los_guidance/los_guidance.py b/guidance/los_guidance/los_guidance/los_guidance.py new file mode 100644 index 00000000..500c8059 --- /dev/null +++ b/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 + diff --git a/guidance/los_guidance/los_guidance/los_guidance_node.py b/guidance/los_guidance/los_guidance/los_guidance_node.py new file mode 100644 index 00000000..ca600a12 --- /dev/null +++ b/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() diff --git a/guidance/los_guidance/package.xml b/guidance/los_guidance/package.xml new file mode 100644 index 00000000..023e7271 --- /dev/null +++ b/guidance/los_guidance/package.xml @@ -0,0 +1,22 @@ + + + + los_guidance + 0.0.0 + TODO: Package description + vortex + MIT + + rclpy + nav_msgs + python-transforms3d-pip + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/guidance/los_guidance/resource/los_guidance b/guidance/los_guidance/resource/los_guidance new file mode 100644 index 00000000..e69de29b diff --git a/guidance/los_guidance/setup.cfg b/guidance/los_guidance/setup.cfg new file mode 100644 index 00000000..8709d00f --- /dev/null +++ b/guidance/los_guidance/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/los_guidance +[install] +install_scripts=$base/lib/los_guidance diff --git a/guidance/los_guidance/setup.py b/guidance/los_guidance/setup.py new file mode 100644 index 00000000..cef8ea5a --- /dev/null +++ b/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='vortex.git@vortexntnu.no', + description='TODO: Package description', + license='MIT', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'los_guidance_node = los_guidance.los_guidance_node:main' + ], + }, +) diff --git a/motion/lqr_controller/LICENSE b/motion/lqr_controller/LICENSE new file mode 100644 index 00000000..30e8e2ec --- /dev/null +++ b/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. diff --git a/motion/lqr_controller/config/lqr_config.yaml b/motion/lqr_controller/config/lqr_config.yaml new file mode 100644 index 00000000..50dc3463 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/motion/lqr_controller/launch/lqr_controller.launch.py b/motion/lqr_controller/launch/lqr_controller.launch.py new file mode 100644 index 00000000..b51a0ab3 --- /dev/null +++ b/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 + ]) diff --git a/motion/lqr_controller/lqr_controller/__init__.py b/motion/lqr_controller/lqr_controller/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/motion/lqr_controller/lqr_controller/asv_model.py b/motion/lqr_controller/lqr_controller/asv_model.py new file mode 100644 index 00000000..09231ce1 --- /dev/null +++ b/motion/lqr_controller/lqr_controller/asv_model.py @@ -0,0 +1,72 @@ +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 new file mode 100644 index 00000000..7c977f8a --- /dev/null +++ b/motion/lqr_controller/lqr_controller/lqr_controller.py @@ -0,0 +1,117 @@ +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 = 30*np.pi/180 # magic init number!!! + 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 + + # def run_ivan_sim(self): + # x_init = np.array([10, -20, 40*np.pi/180, 0, 0, 0]) + # x_ref = np.array([0, 0, self.heading_ref]) + + # look_ahead = 5 # Look ahead distance + + # T = 69.0 # Total simulation time + # dt = 0.01 # Time step + # num_steps = int(T / dt) # Number of time steps + + # # Initialize arrays to store data + # time = np.linspace(0, T, num_steps+1) + # x_history = np.zeros((num_steps+1, self.A.shape[0])) + # u_history = np.zeros((num_steps+1, self.B.shape[1])) + # x_ref_history = np.zeros((num_steps+1, np.shape(x_ref)[0])) + + # cross_track_error_history = np.zeros(num_steps+1) + + # # Simulation loop + # x = x_init # Initial state + # for i in range(num_steps+1): + # #x_ref[i] = reference_function_const(i * dt) # Update the reference at each time step + # # x_ref_history[i, :] = x_ref # Update the reference at each time step + + # # generate reference at the look-ahead distance + # p_asv = np.array([x[0], x[1]]) + # errors = self.R_Pi_p.T @ (p_asv - self.p0) + # along_track_error = errors[0] + # p_los_world = self.R_Pi_p @ np.array([along_track_error + look_ahead, 0]) + self.p0 + + # x_ref[:2] = p_los_world # Update the position reference at each time step + # x_ref_history[i, :] = x_ref + + # u = self.calculate_control_input(x, x_ref, self.K_LQR, self.K_r) # Calculate control input 'u' at each time step + # x = self.asv.RK4_integration_step(x, u, dt) + + # x_history[i] = x # Store state history + # u_history[i] = u # Store control input history + + # # Plot results + # plt.figure(figsize=(12, 8)) + # plt.subplot(3, 1, 1) + # for j in range(3): + # plt.plot(time, x_history[:, j], label=f'State (x_{j+1})') + # plt.plot(time, 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(self.B.shape[1]): + # plt.plot(time, 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(x_history[:,0], x_history[:,1], label=f'Position') + # plt.plot([self.p0[0], self.p1[0]], [self.p0[1], self.p1[1]], 'r-', label='Path') + # plt.xlabel('x') + # plt.ylabel('y') + # plt.legend() + + # plt.tight_layout() + # plt.show() + # plt.figure(1) + # plt.plot(time, cross_track_error_history, label="cross track error") + # plt.axis("equal") + # plt.plot(time, np.zeros_like(time)) + # plt.legend() + # plt.show() + + + + \ No newline at end of file diff --git a/motion/lqr_controller/lqr_controller/lqr_controller_node.py b/motion/lqr_controller/lqr_controller/lqr_controller_node.py new file mode 100644 index 00000000..d1d6246d --- /dev/null +++ b/motion/lqr_controller/lqr_controller/lqr_controller_node.py @@ -0,0 +1,99 @@ +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) + #self.lqr_controller.run_ivan_sim() + + # 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 new file mode 100644 index 00000000..b38062f7 --- /dev/null +++ b/motion/lqr_controller/package.xml @@ -0,0 +1,22 @@ + + + + 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 new file mode 100644 index 00000000..e69de29b diff --git a/motion/lqr_controller/setup.cfg b/motion/lqr_controller/setup.cfg new file mode 100644 index 00000000..e61c4ac2 --- /dev/null +++ b/motion/lqr_controller/setup.cfg @@ -0,0 +1,4 @@ +[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 new file mode 100644 index 00000000..ada29567 --- /dev/null +++ b/motion/lqr_controller/setup.py @@ -0,0 +1,30 @@ +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/vessel_simulator/LICENSE b/motion/vessel_simulator/LICENSE new file mode 100644 index 00000000..30e8e2ec --- /dev/null +++ b/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. diff --git a/motion/vessel_simulator/launch/vessel_simulator.launch.py b/motion/vessel_simulator/launch/vessel_simulator.launch.py new file mode 100644 index 00000000..e9c7d5cb --- /dev/null +++ b/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 + ]) + diff --git a/motion/vessel_simulator/package.xml b/motion/vessel_simulator/package.xml new file mode 100644 index 00000000..00033eb7 --- /dev/null +++ b/motion/vessel_simulator/package.xml @@ -0,0 +1,29 @@ + + + + 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 new file mode 100644 index 00000000..e69de29b diff --git a/motion/vessel_simulator/setup.cfg b/motion/vessel_simulator/setup.cfg new file mode 100644 index 00000000..d28c4cc5 --- /dev/null +++ b/motion/vessel_simulator/setup.cfg @@ -0,0 +1,4 @@ +[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 new file mode 100644 index 00000000..58c3dba8 --- /dev/null +++ b/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='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 new file mode 100644 index 00000000..e69de29b diff --git a/motion/vessel_simulator/vessel_simulator/vessel_simulator_node.py b/motion/vessel_simulator/vessel_simulator/vessel_simulator_node.py new file mode 100644 index 00000000..09a5c340 --- /dev/null +++ b/motion/vessel_simulator/vessel_simulator/vessel_simulator_node.py @@ -0,0 +1,221 @@ +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 on Martin's Thinkpad, will probably go faster for everybody else :) + """) + 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 = [40.0, 40.0] + p2 = [90.0, -20.0] + p3 = [120.0, 50.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() diff --git a/motion/vessel_simulator_realtime/LICENSE b/motion/vessel_simulator_realtime/LICENSE new file mode 100644 index 00000000..30e8e2ec --- /dev/null +++ b/motion/vessel_simulator_realtime/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. diff --git a/motion/vessel_simulator_realtime/cfg/freya_sim_foxglove.json b/motion/vessel_simulator_realtime/cfg/freya_sim_foxglove.json new file mode 100644 index 00000000..602ac170 --- /dev/null +++ b/motion/vessel_simulator_realtime/cfg/freya_sim_foxglove.json @@ -0,0 +1,214 @@ +{ + "configById": { + "3D!45bpewl": { + "cameraState": { + "perspective": true, + "distance": 250.52738551722237, + "phi": 179.99994266987844, + "thetaOffset": -89.99999770360385, + "targetOffset": [ + 73.97173196727483, + 0.23675590015567488, + 1.2349479935931187e-21 + ], + "target": [ + 0, + 0, + 0 + ], + "targetOrientation": [ + 0, + 0, + 0, + 1 + ], + "fovy": 45, + "near": 0.5, + "far": 5000 + }, + "followMode": "follow-pose", + "followTf": "world", + "scene": { + "transforms": { + "lineWidth": 0.5 + }, + "meshUpAxis": "y_up" + }, + "transforms": {}, + "topics": { + "/sensor/seapath/path/ned": { + "visible": true, + "lineWidth": 0.5, + "gradient": [ + "#ffffffff", + "#00be1080" + ] + }, + "/sensor/seapath/posestamped/ned": { + "visible": true, + "axisScale": 10.2, + "type": "axis", + "arrowScale": [ + 10, + 3.15, + 3.15 + ] + }, + "/move_base_simple/goal": { + "visible": false + }, + "/sensor/seapath/state_path/ned": { + "visible": true, + "lineWidth": 1, + "gradient": [ + "#fafafaff", + "#00ff0d80" + ] + }, + "/sensor/seapath/xref_path/ned": { + "visible": true, + "gradient": [ + "#c750cfff", + "#ae19a9ff" + ], + "lineWidth": 0.5 + } + }, + "layers": { + "6bba1d8c-b741-42b0-901c-20e59bfe5fcc": { + "visible": true, + "frameLocked": true, + "label": "Grid", + "instanceId": "6bba1d8c-b741-42b0-901c-20e59bfe5fcc", + "layerId": "foxglove.Grid", + "size": 1000, + "divisions": 100, + "color": "#248eff", + "position": [ + 0, + 0, + 0 + ], + "rotation": [ + 0, + 0, + 0 + ], + "order": 1, + "lineWidth": 0.3 + } + }, + "publish": { + "type": "point", + "poseTopic": "/move_base_simple/goal", + "pointTopic": "/clicked_point", + "poseEstimateTopic": "/initialpose", + "poseEstimateXDeviation": 0.5, + "poseEstimateYDeviation": 0.5, + "poseEstimateThetaDeviation": 0.26179939 + }, + "imageMode": {} + }, + "Plot!41trey": { + "paths": [ + { + "timestampMethod": "receiveTime", + "value": "/sensor/seapath/posestamped/ned.pose.position.x", + "enabled": true + }, + { + "timestampMethod": "receiveTime", + "value": "/controller/lqr/reference.pose.pose.position.x", + "enabled": true + } + ], + "showXAxisLabels": true, + "showYAxisLabels": true, + "showLegend": false, + "legendDisplay": "floating", + "showPlotValuesInLegend": true, + "isSynced": true, + "xAxisVal": "timestamp", + "sidebarDimension": 240, + "followingViewWidth": 30, + "foxglovePanelTitle": "Surge" + }, + "Plot!q6f7ui": { + "paths": [ + { + "timestampMethod": "receiveTime", + "value": "/sensor/seapath/posestamped/ned.pose.position.y", + "enabled": true + }, + { + "timestampMethod": "receiveTime", + "value": "/controller/lqr/reference.pose.pose.position.y", + "enabled": true + } + ], + "showXAxisLabels": true, + "showYAxisLabels": true, + "showLegend": false, + "legendDisplay": "floating", + "showPlotValuesInLegend": true, + "isSynced": true, + "xAxisVal": "timestamp", + "sidebarDimension": 240, + "followingViewWidth": 30, + "foxglovePanelTitle": "Sway" + }, + "Tab!23gzpt8": { + "activeTabIdx": 0, + "tabs": [ + { + "title": "1", + "layout": { + "first": "3D!45bpewl", + "second": { + "first": "Plot!41trey", + "second": { + "first": "Plot!q6f7ui", + "second": "Plot!4ia1dcm", + "direction": "row" + }, + "direction": "row", + "splitPercentage": 29.72972972972973 + }, + "direction": "column", + "splitPercentage": 69.86721144024514 + } + } + ] + }, + "Plot!4ia1dcm": { + "paths": [ + { + "timestampMethod": "receiveTime", + "value": "/sensor/seapath/yaw/ned.data", + "enabled": true + }, + { + "timestampMethod": "receiveTime", + "value": "/sensor/seapath/yaw_ref/ned.data", + "enabled": true + } + ], + "showXAxisLabels": true, + "showYAxisLabels": true, + "showLegend": true, + "legendDisplay": "floating", + "showPlotValuesInLegend": true, + "isSynced": true, + "xAxisVal": "timestamp", + "sidebarDimension": 240, + "followingViewWidth": 30, + "foxglovePanelTitle": "Yaw" + } + }, + "globalVariables": {}, + "userNodes": {}, + "playbackConfig": { + "speed": 1 + }, + "layout": "Tab!23gzpt8" +} \ No newline at end of file diff --git a/motion/vessel_simulator_realtime/launch/vessel_simulator_realtime.launch.py b/motion/vessel_simulator_realtime/launch/vessel_simulator_realtime.launch.py new file mode 100644 index 00000000..6220e4db --- /dev/null +++ b/motion/vessel_simulator_realtime/launch/vessel_simulator_realtime.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_realtime_node = Node( + package='vessel_simulator_realtime', + executable='vessel_simulator_realtime_node', + name='vessel_simulator_realtime_node', + # parameters=[os.path.join(get_package_share_directory('vessel_simulator_realtime'),'config','lqr_config.yaml')], + output='screen', + ) + return LaunchDescription([ + vessel_simulator_realtime_node + ]) + diff --git a/motion/vessel_simulator_realtime/package.xml b/motion/vessel_simulator_realtime/package.xml new file mode 100644 index 00000000..ccddaac6 --- /dev/null +++ b/motion/vessel_simulator_realtime/package.xml @@ -0,0 +1,29 @@ + + + + vessel_simulator_realtime + 0.0.0 + 3DOF marine vessel simulation and visualization package. + BenG + 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_realtime/resource/vessel_simulator_realtime b/motion/vessel_simulator_realtime/resource/vessel_simulator_realtime new file mode 100644 index 00000000..e69de29b diff --git a/motion/vessel_simulator_realtime/setup.cfg b/motion/vessel_simulator_realtime/setup.cfg new file mode 100644 index 00000000..8076957e --- /dev/null +++ b/motion/vessel_simulator_realtime/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/vessel_simulator_realtime +[install] +install_scripts=$base/lib/vessel_simulator_realtime diff --git a/motion/vessel_simulator_realtime/setup.py b/motion/vessel_simulator_realtime/setup.py new file mode 100644 index 00000000..3dda1e92 --- /dev/null +++ b/motion/vessel_simulator_realtime/setup.py @@ -0,0 +1,29 @@ +import os +from glob import glob +from setuptools import find_packages, setup + +package_name = 'vessel_simulator_realtime' + +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='BenG', + maintainer_email='benjamin.visockis@gmail.com', + description='3DOF marine vessel simulation and visualization package.', + license='MIT', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'vessel_simulator_realtime_node = vessel_simulator_realtime.vessel_simulator_realtime_node:main' + ], + }, +) diff --git a/motion/vessel_simulator_realtime/vessel_simulator_realtime/TF2Broadcaster.py b/motion/vessel_simulator_realtime/vessel_simulator_realtime/TF2Broadcaster.py new file mode 100644 index 00000000..fe45d10e --- /dev/null +++ b/motion/vessel_simulator_realtime/vessel_simulator_realtime/TF2Broadcaster.py @@ -0,0 +1,79 @@ +from geometry_msgs.msg import TransformStamped +from rclpy.node import Node +from tf2_ros import TransformBroadcaster +import math +import numpy as np + +def quaternion_from_euler(ai, aj, ak): + ai /= 2.0 + aj /= 2.0 + ak /= 2.0 + ci = math.cos(ai) + si = math.sin(ai) + cj = math.cos(aj) + sj = math.sin(aj) + ck = math.cos(ak) + sk = math.sin(ak) + cc = ci*ck + cs = ci*sk + sc = si*ck + ss = si*sk + + q = np.empty((4, )) + q[0] = cj*sc - sj*cs + q[1] = cj*ss + sj*cc + q[2] = cj*cs - sj*sc + q[3] = cj*cc + sj*ss + + return q + + +class TF2Broadcaster(Node): + def __init__(self, frame_id): + super().__init__('tf2_broadcaster') + self.tf_broadcaster = TransformBroadcaster(self) + + self.own_vessel_frame_id = frame_id + + def fixed_frame_broadcaster(self): + t = TransformStamped() + + t.header.stamp = self.get_clock().now().to_msg() + t.header.frame_id = 'map' + t.child_frame_id = 'world' + t.transform.translation.x = 0.0 + t.transform.translation.y = 0.0 + t.transform.translation.z = 0.0 + t.transform.rotation.x = 0.0 + t.transform.rotation.y = 0.0 + t.transform.rotation.z = 0.0 + t.transform.rotation.w = 0.0 + + self.tf_broadcaster.sendTransform(t) + + def handle_pose(self, state): + t = TransformStamped() + + # Read message content and assign it to + # corresponding tf variables + t.header.stamp = self.get_clock().now().to_msg() + t.header.frame_id = 'world' + t.child_frame_id = self.own_vessel_frame_id + + # Turtle only exists in 2D, thus we get x and y translation + # coordinates from the message and set the z coordinate to 0 + t.transform.translation.x = state[0] + t.transform.translation.y = state[1] + t.transform.translation.z = 0.0 + + # For the same reason, turtle can only rotate around one axis + # and this why we set rotation in x and y to 0 and obtain + # rotation in z axis from the message + q = quaternion_from_euler(0, 0, state[2]) + t.transform.rotation.x = q[0] + t.transform.rotation.y = q[1] + t.transform.rotation.z = q[2] + t.transform.rotation.w = q[3] + + # Send the transformation + self.tf_broadcaster.sendTransform(t) \ No newline at end of file diff --git a/motion/vessel_simulator_realtime/vessel_simulator_realtime/VesselVisualizerNode.py b/motion/vessel_simulator_realtime/vessel_simulator_realtime/VesselVisualizerNode.py new file mode 100644 index 00000000..ab869fab --- /dev/null +++ b/motion/vessel_simulator_realtime/vessel_simulator_realtime/VesselVisualizerNode.py @@ -0,0 +1,137 @@ +import rclpy +from rclpy.node import Node + +from std_msgs.msg import Int64, Float32 +from geometry_msgs.msg import Wrench, PoseStamped +from nav_msgs.msg import Odometry, Path + +import numpy as np +import time + +from vessel_simulator_realtime.conversions import * +from vessel_simulator_realtime.simulation import * +from vessel_simulator_realtime.TF2Broadcaster import TF2Broadcaster + +class VesselVisualizerNode(Node): + def __init__(self): + super().__init__("vessel_visualizer_node") + + # Init TF2 brpadcaster for frames and tfs + self.own_vessel_frame_id = "asv_pose" + self.tf_broadcaster = TF2Broadcaster(self.own_vessel_frame_id) + + # 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.posestamped_publisher_ = self.create_publisher(PoseStamped, "/sensor/seapath/posestamped/ned", 1) + self.state_path_publisher_ = self.create_publisher(Path, "/sensor/seapath/state_path/ned", 1) + self.xref_path_publisher_ = self.create_publisher(Path, "/sensor/seapath/xref_path/ned", 1) + + self.yaw_ref_publisher_ = self.create_publisher(Float32, "/sensor/seapath/yaw_ref/ned", 1) + self.yaw_publisher_ = self.create_publisher(Float32, "/sensor/seapath/yaw/ned", 1) + + self.test_publisher = self.create_publisher(Int64, "/test1", 1) + + # create timer + timer_period = 0.1 # seconds + self.timer = self.create_timer(timer_period, self.timer_callback) + + # Init state and control variables + self.state = np.array([10, -20, 40*np.pi/180, 0, 0, 0]) + self.x_ref = np.array([0, 0, 50*np.pi/180, 0, 0, 0]) # Note to self: make x_ref have 6 states + self.u = np.array([0, 0, 0]) + + # Init historical pose (path) + self.state_path = Path() + self.xref_path = Path() + + self.N = 5000 # Example length + # for _ in range(N): + # pose = PoseStamped() + # pose.header.frame_id = "world" + # pose.header.stamp = self.get_clock().now().to_msg() + # # Initialize pose here if needed + # self.state_path.poses.append(pose) + # self.xref_path.poses.append(pose) + + # Init simulation parameters + 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]) + + realtime_factor = 10.0 + self.dt = timer_period*realtime_factor + self.num_steps_simulated = 0 + + # Publish initial state + self.state_publisher_.publish(state_to_odometrymsg(self.state)) + + def timer_callback(self): + self.simulate_step() + msg = Int64() + msg.data = self.num_steps_simulated + self.test_publisher.publish(msg) + + def guidance_cb(self, msg): + self.x_ref = odometrymsg_to_state(msg) + self.yaw_ref_publisher_.publish(Float32(data=self.x_ref[2])) + + def wrench_input_cb(self, msg): + self.u = np.array([msg.force.x, msg.force.y, msg.torque.z]) + + # def update_path_element(self, n, new_pose): + # if n < len(self.state_path.poses): + # self.state_path.poses[n] = new_pose + # else: + # # Shift elements and append new_pose + # self.state_path.poses.pop(0) + # self.state_path.poses.append(new_pose) + + def update_path_element(self, path, new_pose, N): + if len(path.poses) < N: + path.poses.append(new_pose) + else: + # Shift elements and append new_pose + path.poses.pop(0) + path.poses.append(new_pose) + + def simulate_step(self): + # Integrate a step forward using RK4 + x_next = RK4_integration_step(self.M_inv, self.D, self.state, self.u, self.dt) + + # Update state + self.state = x_next + + # Pub odom + odometry_msg = state_to_odometrymsg(x_next) + self.state_publisher_.publish(odometry_msg) + + # Pub Posestamped + posestamped_msg = state_to_posestamped(x_next, "world", self.get_clock().now().to_msg()) + self.posestamped_publisher_.publish(posestamped_msg) + self.yaw_publisher_.publish(Float32(data=x_next[2])) + + # Pub Paths + self.state_path.header.frame_id = "world" + self.state_path.header.stamp = self.get_clock().now().to_msg() + + self.update_path_element(self.state_path, posestamped_msg, self.N) + self.state_path_publisher_.publish(self.state_path) + + xref_msg = state_to_posestamped(self.x_ref, "world", self.get_clock().now().to_msg()) + self.xref_path.header.frame_id = "world" + self.xref_path.header.stamp = self.get_clock().now().to_msg() + + self.update_path_element(self.xref_path, xref_msg, self.N) + self.xref_path_publisher_.publish(self.xref_path) + + # Pub frame and odom tf + self.tf_broadcaster.fixed_frame_broadcaster() + self.tf_broadcaster.handle_pose(x_next) + + # Update current sim step + self.num_steps_simulated += 1 diff --git a/motion/vessel_simulator_realtime/vessel_simulator_realtime/__init__.py b/motion/vessel_simulator_realtime/vessel_simulator_realtime/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/motion/vessel_simulator_realtime/vessel_simulator_realtime/conversions.py b/motion/vessel_simulator_realtime/vessel_simulator_realtime/conversions.py new file mode 100644 index 00000000..92f14cfb --- /dev/null +++ b/motion/vessel_simulator_realtime/vessel_simulator_realtime/conversions.py @@ -0,0 +1,59 @@ +import numpy as np + +from transforms3d.euler import euler2quat, quat2euler +from nav_msgs.msg import Odometry +from geometry_msgs.msg import PoseStamped + + +def odometrymsg_to_state(msg): + x = msg.pose.pose.position.x + y = msg.pose.pose.position.y + orientation_q = msg.pose.pose.orientation + orientation_list = [ + orientation_q.w, orientation_q.x, orientation_q.y, orientation_q.z + ] + + # Convert quaternion to Euler angles, extract yaw + yaw = quat2euler(orientation_list)[2] + + vx = msg.twist.twist.linear.x + vy = msg.twist.twist.linear.y + vyaw = msg.twist.twist.angular.z + + state = np.array([x, y, yaw, vx, vy, vyaw]) + return state + +def state_to_odometrymsg(state): + orientation_list_next = euler2quat(0, 0, state[2]) + + odometry_msg = Odometry() + odometry_msg.pose.pose.position.x = state[0] + odometry_msg.pose.pose.position.y = state[1] + odometry_msg.pose.pose.position.z = 0.0 + odometry_msg.pose.pose.orientation.w = orientation_list_next[0] + odometry_msg.pose.pose.orientation.x = orientation_list_next[1] + odometry_msg.pose.pose.orientation.y = orientation_list_next[2] + odometry_msg.pose.pose.orientation.z = orientation_list_next[3] + + return odometry_msg + +def state_to_posestamped(state, frame_id, stamp): + orientation_list_next = euler2quat(0, 0, state[2]) + + posestamped_msg = PoseStamped() + + posestamped_msg.header.frame_id = frame_id + posestamped_msg.header.stamp = stamp + + posestamped_msg.pose.position.x = state[0] + posestamped_msg.pose.position.y = state[1] + posestamped_msg.pose.position.z = 0.0 + posestamped_msg.pose.orientation.w = orientation_list_next[0] + posestamped_msg.pose.orientation.x = orientation_list_next[1] + posestamped_msg.pose.orientation.y = orientation_list_next[2] + posestamped_msg.pose.orientation.z = orientation_list_next[3] + + return posestamped_msg + +def ssa(angle): + return (angle + np.pi) % (2 * np.pi) - np.pi \ No newline at end of file diff --git a/motion/vessel_simulator_realtime/vessel_simulator_realtime/simulation.py b/motion/vessel_simulator_realtime/vessel_simulator_realtime/simulation.py new file mode 100644 index 00000000..2224d261 --- /dev/null +++ b/motion/vessel_simulator_realtime/vessel_simulator_realtime/simulation.py @@ -0,0 +1,37 @@ +import numpy as np + +def state_dot(M_inv: np.ndarray, D: np.ndarray, state: np.ndarray, tau_actuation: np.ndarray, V_current: np.ndarray = np.zeros(2)) -> np.ndarray: + """ + Calculate the derivative of the state using the non-linear kinematics + """ + heading = state[2] + + J = np.array( + [[np.cos(heading), -np.sin(heading), 0], + [np.sin(heading), np.cos(heading), 0], + [0, 0, 1]] + ) + + A = np.zeros((6,6)) + + A[:3,3:] = J + A[3:, 3:] = - M_inv @ D + + B = np.zeros((6,3)) + B[3:,:] = M_inv + + x_dot = A @ state + B @ tau_actuation + x_dot[0:2] += V_current # add current drift term at velocity level + + return x_dot + +def RK4_integration_step(M_inv: np.ndarray, D: np.ndarray, x: np.ndarray, u: np.ndarray, dt: float) -> np.ndarray: + # integration scheme for simulation, implements the Runge-Kutta 4 integrator + k1 = state_dot(M_inv, D, x, u) + k2 = state_dot(M_inv, D, x+dt/2*k1, u) + k3 = state_dot(M_inv, D, x+dt/2*k2, u) + k4 = state_dot(M_inv, D, x+dt*k3, u) + + x_next = x + dt/6*(k1+2*k2+2*k3+k4) + + return x_next \ No newline at end of file diff --git a/motion/vessel_simulator_realtime/vessel_simulator_realtime/vessel_simulator_realtime_node.py b/motion/vessel_simulator_realtime/vessel_simulator_realtime/vessel_simulator_realtime_node.py new file mode 100644 index 00000000..ab4d1616 --- /dev/null +++ b/motion/vessel_simulator_realtime/vessel_simulator_realtime/vessel_simulator_realtime_node.py @@ -0,0 +1,16 @@ +import rclpy +from vessel_simulator_realtime.VesselVisualizerNode import VesselVisualizerNode + + +def main(args=None): + rclpy.init(args=args) + node = VesselVisualizerNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + node.destroy_node() + rclpy.shutdown() + +if __name__ == "__main__": + main()