diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py index e794e84b..f319dc60 100755 --- a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py +++ b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py @@ -7,7 +7,7 @@ from vortex_msgs.srv import Waypoint from nav_msgs.msg import Odometry from transforms3d.euler import quat2euler -from std_msgs.msg import Bool +from std_msgs.msg import Bool, Int32 from hybridpath_guidance.hybridpath import HybridPathGenerator, HybridPathSignals @@ -27,6 +27,7 @@ def __init__(self): self.eta_subscriber_ = self.create_subscription(Odometry, '/sensor/seapath/odom/ned', self.eta_callback, 1) self.guidance_publisher = self.create_publisher(HybridpathReference, 'guidance/hybridpath/reference', 1) self.switching_publisher = self.create_publisher(Bool, 'guidance/hybridpath/finished', 10) + self.switching_criteria_subscriber = self.create_subscription(Int32, 'guidance/hybridpath/switching', self.switching_callback, 10) # Get parameters self.lambda_val = self.get_parameter('hybridpath_guidance.lambda_val').get_parameter_value().double_value @@ -35,6 +36,8 @@ def __init__(self): self.mu = self.get_parameter('hybridpath_guidance.mu').get_parameter_value().double_value self.eta = np.zeros(3) + self.switching_waypoint = 1 + # Flags for logging self.waypoints_received = False self.waiting_message_printed = False @@ -61,6 +64,9 @@ def waypoint_callback(self, request, response): response.success = True return response + def switching_callback(self, msg: Int32): + self.switching_waypoint = msg.data + def eta_callback(self, msg: Odometry): self.eta = self.odom_to_eta(msg) diff --git a/mission/mission_planner/mission_planner/mission_planner.py b/mission/mission_planner/mission_planner/mission_planner.py index b0405299..15c6ff8b 100755 --- a/mission/mission_planner/mission_planner/mission_planner.py +++ b/mission/mission_planner/mission_planner/mission_planner.py @@ -4,7 +4,7 @@ from rclpy.node import Node from vortex_msgs.srv import MissionParameters#, s from geometry_msgs.msg import Point -from std_msgs.msg import String, Bool +from std_msgs.msg import String, Bool, Int32 class MissionPlannerClient(Node): """ @@ -30,6 +30,7 @@ def __init__(self): self.active_controller_publisher = self.create_publisher(String, 'mission/controller', 10) self.hybridpath_guidance_subscriber = self.create_subscription(Bool, 'guidance/hybridpath/finished', self.guidance_callback, 10) self.active_controller_timer = self.create_timer(1.0, self.timer_callback) + self.switching_waypoint_publisher = self.create_publisher(Int32, 'guidance/hybridpath/switching', 10) def timer_callback(self): msg = String() @@ -37,6 +38,11 @@ def timer_callback(self): self.active_controller_publisher.publish(msg) self.get_logger().info(f'Publishing: {self.active_controller}') + # Switching criteria + msg = Int32() + msg.data = 1 + self.switching_waypoint_publisher.publish(msg) + def guidance_callback(self, msg): if msg.data == True: self.active_controller = 'DP' diff --git a/motion/hybridpath_controller/config/hybridpath_controller_config.yaml b/motion/hybridpath_controller/config/hybridpath_controller_config.yaml index e7ac823a..7deeec2f 100644 --- a/motion/hybridpath_controller/config/hybridpath_controller_config.yaml +++ b/motion/hybridpath_controller/config/hybridpath_controller_config.yaml @@ -1,5 +1,5 @@ /**: ros__parameters: hybridpath_controller: - K1_diag: [10.0, 10.0, 10.0] # First gain matrix - K2_diag: [60.0, 60.0, 60.0] # Second gain matrix \ No newline at end of file + K1_diag: [1.0, 1.0, 1.0] # First gain matrix + K2_diag: [60.0, 59.0, 47.0] # Second gain matrix \ No newline at end of file diff --git a/motion/lqr_controller/CMakeLists.txt b/motion/lqr_controller/CMakeLists.txt new file mode 100644 index 00000000..cafa3ad4 --- /dev/null +++ b/motion/lqr_controller/CMakeLists.txt @@ -0,0 +1,27 @@ +cmake_minimum_required(VERSION 3.8) +project(lqr_controller) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake_python REQUIRED) +find_package(rclpy REQUIRED) +find_package(vortex_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) + +ament_python_install_package(${PROJECT_NAME}) + +install(DIRECTORY + launch + config + DESTINATION share/${PROJECT_NAME} +) + +install(PROGRAMS +lqr_controller/lqr_controller_node.py + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() \ No newline at end of file diff --git a/motion/lqr_controller/config/lqr_config.yaml b/motion/lqr_controller/config/lqr_config.yaml new file mode 100644 index 00000000..ac497221 --- /dev/null +++ b/motion/lqr_controller/config/lqr_config.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + lqr_controller: + Q: [20.0, 20.0, 1.0, 1.0, 1.0, 1.0] # State costs for [x, y, heading, surge, sway, yaw] + R: [1.0, 1.0, 1.0] # Control cost weight \ No newline at end of file diff --git a/motion/lqr_controller/launch/lqr_controller.launch.py b/motion/lqr_controller/launch/lqr_controller.launch.py new file mode 100644 index 00000000..03ceca10 --- /dev/null +++ b/motion/lqr_controller/launch/lqr_controller.launch.py @@ -0,0 +1,19 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + lqr_controller_node = Node( + package='lqr_controller', + executable='lqr_controller_node.py', + name='lqr_controller_node', + parameters=[ + os.path.join(get_package_share_directory('lqr_controller'),'config','lqr_config.yaml'), + os.path.join(get_package_share_directory('asv_setup'), 'config', 'robots', 'freya.yaml') + ], + output='screen', + ) + return LaunchDescription([ + lqr_controller_node + ]) diff --git a/motion/lqr_controller/lqr_controller/__init__.py b/motion/lqr_controller/lqr_controller/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/motion/lqr_controller/lqr_controller/conversions.py b/motion/lqr_controller/lqr_controller/conversions.py new file mode 100644 index 00000000..92f14cfb --- /dev/null +++ b/motion/lqr_controller/lqr_controller/conversions.py @@ -0,0 +1,59 @@ +import numpy as np + +from transforms3d.euler import euler2quat, quat2euler +from nav_msgs.msg import Odometry +from geometry_msgs.msg import PoseStamped + + +def odometrymsg_to_state(msg): + x = msg.pose.pose.position.x + y = msg.pose.pose.position.y + orientation_q = msg.pose.pose.orientation + orientation_list = [ + orientation_q.w, orientation_q.x, orientation_q.y, orientation_q.z + ] + + # Convert quaternion to Euler angles, extract yaw + yaw = quat2euler(orientation_list)[2] + + vx = msg.twist.twist.linear.x + vy = msg.twist.twist.linear.y + vyaw = msg.twist.twist.angular.z + + state = np.array([x, y, yaw, vx, vy, vyaw]) + return state + +def state_to_odometrymsg(state): + orientation_list_next = euler2quat(0, 0, state[2]) + + odometry_msg = Odometry() + odometry_msg.pose.pose.position.x = state[0] + odometry_msg.pose.pose.position.y = state[1] + odometry_msg.pose.pose.position.z = 0.0 + odometry_msg.pose.pose.orientation.w = orientation_list_next[0] + odometry_msg.pose.pose.orientation.x = orientation_list_next[1] + odometry_msg.pose.pose.orientation.y = orientation_list_next[2] + odometry_msg.pose.pose.orientation.z = orientation_list_next[3] + + return odometry_msg + +def state_to_posestamped(state, frame_id, stamp): + orientation_list_next = euler2quat(0, 0, state[2]) + + posestamped_msg = PoseStamped() + + posestamped_msg.header.frame_id = frame_id + posestamped_msg.header.stamp = stamp + + posestamped_msg.pose.position.x = state[0] + posestamped_msg.pose.position.y = state[1] + posestamped_msg.pose.position.z = 0.0 + posestamped_msg.pose.orientation.w = orientation_list_next[0] + posestamped_msg.pose.orientation.x = orientation_list_next[1] + posestamped_msg.pose.orientation.y = orientation_list_next[2] + posestamped_msg.pose.orientation.z = orientation_list_next[3] + + return posestamped_msg + +def ssa(angle): + return (angle + np.pi) % (2 * np.pi) - np.pi \ No newline at end of file diff --git a/motion/lqr_controller/lqr_controller/lqr_controller.py b/motion/lqr_controller/lqr_controller/lqr_controller.py new file mode 100644 index 00000000..7c7b6f67 --- /dev/null +++ b/motion/lqr_controller/lqr_controller/lqr_controller.py @@ -0,0 +1,39 @@ +import numpy as np +from scipy.linalg import solve_continuous_are + +class LQRController: + def __init__(self, M: float, D: list[float], Q: list[float], R: list[float], heading_ref: float = 0.0): + self.M = M + self.M_inv = np.linalg.inv(self.M) + self.D = D + + self.A = np.zeros((6,6)) + self.B = np.zeros((6,3)) + self.C = np.zeros((3,6)) + self.C[:3, :3] = np.eye(3) + + self.Q = np.diag(Q) + print(f"Q: {self.Q}") + self.R = np.diag(R) + + self.linearize_model(heading_ref) + + def calculate_control_input(self, x, x_ref): + tau = -self.K_LQR @ x + self.K_r @ x_ref + return tau + + def linearize_model(self, heading: float) -> tuple[np.ndarray, np.ndarray]: + R = np.array( + [[np.cos(heading), -np.sin(heading), 0], + [np.sin(heading), np.cos(heading), 0], + [0, 0, 1]] + ) + + self.A[:3,3:] = R + self.A[3:, 3:] = - self.M_inv @ self.D + + self.B[3:,:] = self.M_inv + + self.P = solve_continuous_are(self.A, self.B, self.Q, self.R) + self.K_LQR = np.dot(np.dot(np.linalg.inv(self.R), self.B.T), self.P) + self.K_r = np.linalg.inv(self.C@np.linalg.inv(self.B @ self.K_LQR - self.A) @ self.B) \ No newline at end of file diff --git a/motion/lqr_controller/lqr_controller/lqr_controller_node.py b/motion/lqr_controller/lqr_controller/lqr_controller_node.py new file mode 100644 index 00000000..91e8ac28 --- /dev/null +++ b/motion/lqr_controller/lqr_controller/lqr_controller_node.py @@ -0,0 +1,90 @@ +#!/usr/bin/env python3 + +import rclpy +import numpy as np +from rclpy.node import Node +from geometry_msgs.msg import Wrench +from nav_msgs.msg import Odometry +from lqr_controller.lqr_controller import LQRController +from lqr_controller.conversions import odometrymsg_to_state +from time import sleep +from std_msgs.msg import String + +from rclpy.qos import QoSProfile, qos_profile_sensor_data, QoSReliabilityPolicy + +qos_profile = QoSProfile(depth=1, history=qos_profile_sensor_data.history, + reliability=QoSReliabilityPolicy.BEST_EFFORT) + +class LQRControllerNode(Node): + def __init__(self): + super().__init__("lqr_controller_node") + + self.declare_parameters( + namespace='', + parameters=[ + ('lqr_controller.Q', [10.0, 10.0, 5.0, 0.1, 1.0, 5.0]), + ('lqr_controller.R', [1.0, 1.0, 1.0]), + ('physical.inertia_matrix', [90.5, 0.0, 0.0, 0.0, 167.5, 12.25, 0.0, 12.25, 42.65]), + ('physical.damping_matrix_diag', [77.55, 162.5, 42.65]) + ]) + + self.state_subscriber_ = self.create_subscription(Odometry, "/sensor/seapath/odom/ned", self.state_cb, qos_profile=qos_profile) + self.guidance_subscriber_ = self.create_subscription(Odometry, "guidance/dp/reference", self.guidance_cb, 1) + self.wrench_publisher_ = self.create_publisher(Wrench, "thrust/wrench_input", 1) + + self.active_controller_subscriber = self.create_subscription(String, 'mission/controller', self.active_controller_callback, 10) + self.active_controller = False + + Q = self.get_parameter('lqr_controller.Q').get_parameter_value().double_array_value + R = self.get_parameter('lqr_controller.R').get_parameter_value().double_array_value + D_diag = self.get_parameter('physical.damping_matrix_diag').get_parameter_value().double_array_value + M = self.get_parameter('physical.inertia_matrix').get_parameter_value().double_array_value + + D = np.diag(D_diag) + M = np.reshape(M, (3, 3)) + + heading_ref = 0.0 + + self.LQR = LQRController(M, D, Q, R, heading_ref) + + self.x_ref = [5, 10, 0] + self.state = [0, 0, 0, 0, 0, 0] + + self.enabled = False + + controller_period = 0.1 + self.controller_timer_ = self.create_timer(controller_period, self.controller_callback) + + self.get_logger().info("lqr_controller_node started") + + def state_cb(self, msg): + self.state = odometrymsg_to_state(msg) + + def guidance_cb(self, msg): + self.x_ref = odometrymsg_to_state(msg)[:3] + + def controller_callback(self): + if hasattr(self, 'state') and hasattr(self, 'x_ref') and self.active_controller: + self.LQR.linearize_model(self.state[2]) + control_input = self.LQR.calculate_control_input(self.state, self.x_ref) + wrench_msg = Wrench() + wrench_msg.force.x = control_input[0] + wrench_msg.force.y = control_input[1] + wrench_msg.torque.z = control_input[2] + self.wrench_publisher_.publish(wrench_msg) + + def active_controller_callback(self, msg): + if msg.data == 'DP': + self.active_controller = True + else: + self.active_controller = False + +def main(args=None): + rclpy.init(args=args) + node = LQRControllerNode() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + +if __name__ == "__main__": + main() diff --git a/motion/lqr_controller/package.xml b/motion/lqr_controller/package.xml new file mode 100644 index 00000000..55cd3a82 --- /dev/null +++ b/motion/lqr_controller/package.xml @@ -0,0 +1,27 @@ + + + + lqr_controller + 0.0.0 + This is an implementation of the LQR DP controller for the ASV + vortex + MIT + + ament_cmake_python + + rclpy + python-transforms3d-pip + nav_msgs + geometry_msgs + vortex_msgs + std_msgs + numpy + matplotlib + scipy + + python3-pytest + + + ament_cmake + + diff --git a/simulation/asv_simulator/CMakeLists.txt b/simulation/asv_simulator/CMakeLists.txt new file mode 100644 index 00000000..6cb6a125 --- /dev/null +++ b/simulation/asv_simulator/CMakeLists.txt @@ -0,0 +1,27 @@ +cmake_minimum_required(VERSION 3.8) +project(asv_simulator) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake_python REQUIRED) +find_package(rclpy REQUIRED) +find_package(vortex_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) + +ament_python_install_package(${PROJECT_NAME}) + +install(DIRECTORY + launch + config + DESTINATION share/${PROJECT_NAME} +) + +install(PROGRAMS +asv_simulator/asv_simulator_node.py + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() \ No newline at end of file diff --git a/simulation/asv_simulator/README.md b/simulation/asv_simulator/README.md new file mode 100755 index 00000000..846f3a9f --- /dev/null +++ b/simulation/asv_simulator/README.md @@ -0,0 +1,16 @@ +# ASV simulator +This package provides the implementation of the asv_simulator for the Vortex ASV. + +## Usage + +Launch controller and guidance `ros2 launch asv_setup hybridpath.launch.py` +Launch the asv_simulator `ros2 launch asv_simulator asv_simulator.launch.py` + +## Configuration + +You can configure the settings of the simulator and waypoints to be used by modifying the parameters in the `config` directory. + +## Foxglove + +To visualize the simulation in Foxglove, launch foxglove bridge `ros2 run foxglove_bridge foxglove_bridge` + diff --git a/simulation/asv_simulator/asv_simulator/TF2Broadcaster.py b/simulation/asv_simulator/asv_simulator/TF2Broadcaster.py new file mode 100644 index 00000000..fe45d10e --- /dev/null +++ b/simulation/asv_simulator/asv_simulator/TF2Broadcaster.py @@ -0,0 +1,79 @@ +from geometry_msgs.msg import TransformStamped +from rclpy.node import Node +from tf2_ros import TransformBroadcaster +import math +import numpy as np + +def quaternion_from_euler(ai, aj, ak): + ai /= 2.0 + aj /= 2.0 + ak /= 2.0 + ci = math.cos(ai) + si = math.sin(ai) + cj = math.cos(aj) + sj = math.sin(aj) + ck = math.cos(ak) + sk = math.sin(ak) + cc = ci*ck + cs = ci*sk + sc = si*ck + ss = si*sk + + q = np.empty((4, )) + q[0] = cj*sc - sj*cs + q[1] = cj*ss + sj*cc + q[2] = cj*cs - sj*sc + q[3] = cj*cc + sj*ss + + return q + + +class TF2Broadcaster(Node): + def __init__(self, frame_id): + super().__init__('tf2_broadcaster') + self.tf_broadcaster = TransformBroadcaster(self) + + self.own_vessel_frame_id = frame_id + + def fixed_frame_broadcaster(self): + t = TransformStamped() + + t.header.stamp = self.get_clock().now().to_msg() + t.header.frame_id = 'map' + t.child_frame_id = 'world' + t.transform.translation.x = 0.0 + t.transform.translation.y = 0.0 + t.transform.translation.z = 0.0 + t.transform.rotation.x = 0.0 + t.transform.rotation.y = 0.0 + t.transform.rotation.z = 0.0 + t.transform.rotation.w = 0.0 + + self.tf_broadcaster.sendTransform(t) + + def handle_pose(self, state): + t = TransformStamped() + + # Read message content and assign it to + # corresponding tf variables + t.header.stamp = self.get_clock().now().to_msg() + t.header.frame_id = 'world' + t.child_frame_id = self.own_vessel_frame_id + + # Turtle only exists in 2D, thus we get x and y translation + # coordinates from the message and set the z coordinate to 0 + t.transform.translation.x = state[0] + t.transform.translation.y = state[1] + t.transform.translation.z = 0.0 + + # For the same reason, turtle can only rotate around one axis + # and this why we set rotation in x and y to 0 and obtain + # rotation in z axis from the message + q = quaternion_from_euler(0, 0, state[2]) + t.transform.rotation.x = q[0] + t.transform.rotation.y = q[1] + t.transform.rotation.z = q[2] + t.transform.rotation.w = q[3] + + # Send the transformation + self.tf_broadcaster.sendTransform(t) \ No newline at end of file diff --git a/simulation/asv_simulator/asv_simulator/__init__.py b/simulation/asv_simulator/asv_simulator/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/simulation/asv_simulator/asv_simulator/asv_simulator_node.py b/simulation/asv_simulator/asv_simulator/asv_simulator_node.py new file mode 100644 index 00000000..e9c4a5b4 --- /dev/null +++ b/simulation/asv_simulator/asv_simulator/asv_simulator_node.py @@ -0,0 +1,241 @@ +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node + +from std_msgs.msg import Float32 +from geometry_msgs.msg import Wrench, PoseStamped, Point +from nav_msgs.msg import Odometry, Path +from vortex_msgs.msg import HybridpathReference +from vortex_msgs.srv import Waypoint + +import matplotlib.pyplot as plt +import numpy as np + +from asv_simulator.conversions import * +from asv_simulator.simulation import * +from asv_simulator.TF2Broadcaster import TF2Broadcaster + +class ASVSimulatorNode(Node): + def __init__(self): + super().__init__('asv_simulator_node') + + self.declare_parameters( + namespace='', + parameters=[ + ('asv_sim.plot_matplotlib_enabled', True), + ('asv_sim.progress_bar_enabled', True), + ('asv_sim.hybridpath_path_following', True), + ('asv_sim.T', 200), + ('asv_sim.waypoints', [0.0, 0.0]), + ('physical.inertia_matrix', [50.0, 50.0, 50.0]), + ('physical.damping_matrix_diag', [10.0, 10.0, 5.0]), + ]) + + self.plot_matplotlib_enabled = self.get_parameter('asv_sim.plot_matplotlib_enabled').get_parameter_value().bool_value + self.progress_bar_enabled = self.get_parameter('asv_sim.progress_bar_enabled').get_parameter_value().bool_value + self.hybridpath_path_following = self.get_parameter('asv_sim.hybridpath_path_following').get_parameter_value().bool_value + self.T = self.get_parameter('asv_sim.T').get_parameter_value().integer_value + waypoints_param = self.get_parameter('asv_sim.waypoints').get_parameter_value().double_array_value + self.waypoints = [Point(x=waypoints_param[i], y=waypoints_param[i+1], z=0.0) for i in range(0, len(waypoints_param), 2)] + D_diag = self.get_parameter('physical.damping_matrix_diag').get_parameter_value().double_array_value + M = self.get_parameter('physical.inertia_matrix').get_parameter_value().double_array_value + + # Init simulation parameters + self.D = np.diag(D_diag) + self.M = np.reshape(M, (3, 3)) + self.M_inv = np.linalg.inv(self.M) + + # Init TF2 broadcaster for frames and tfs + self.own_vessel_frame_id = "asv_pose" + self.tf_broadcaster = TF2Broadcaster(self.own_vessel_frame_id) + + # subscribe to thrust input and guidance + self.wrench_subscriber_ = self.create_subscription(Wrench, "thrust/wrench_input", self.wrench_input_cb, 1) + self.guidance_subscriber_ = self.create_subscription(HybridpathReference, 'guidance/hybridpath/reference', self.guidance_cb, 1) + + # publish state (seapath) + self.state_publisher_ = self.create_publisher(Odometry, "/sensor/seapath/odom/ned", 1) + self.posestamped_publisher_ = self.create_publisher(PoseStamped, "/sensor/seapath/posestamped/ned", 1) + self.state_path_publisher_ = self.create_publisher(Path, "/sensor/seapath/state_path/ned", 1) + self.xref_path_publisher_ = self.create_publisher(Path, "/sensor/seapath/xref_path/ned", 1) + + self.yaw_ref_publisher_ = self.create_publisher(Float32, "/sensor/seapath/yaw_ref/ned", 1) + self.yaw_publisher_ = self.create_publisher(Float32, "/sensor/seapath/yaw/ned", 1) + + # send waypoints to guidance if hybridpath_path_following is enabled + if self.hybridpath_path_following: + self.waypoint_client = self.create_client(Waypoint, 'waypoint_list') + while not self.waypoint_client.wait_for_service(timeout_sec=1.0): + self.get_logger().info('service not available, waiting again...') + self.send_waypoint_request() + + # create timer + timer_period = 0.1 # seconds + self.timer = self.create_timer(timer_period, self.timer_callback) + + # Init state and control variables + self.state = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) + self.x_ref = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) + self.tau = np.array([0.0, 0.0, 0.0]) + + # Init historical pose (path) + self.state_path = Path() + self.xref_path = Path() + self.tau_history = np.zeros((0, 3)) + + realtime_factor = 10.0 + self.dt = timer_period*realtime_factor + self.num_steps_simulated = 0 + + self.N = 5000 # max Path length + self.max_steps = int(self.T/self.dt) + self.sim_finished = False + + # Publish initial state + self.state_publisher_.publish(state_to_odometrymsg(self.state)) + + self.get_logger().info("ASV simulator node initialized with parameters: \n" + + "Simulation time T = " + str(self.T) + "\n" + + "Matplotlib plotting enabled: " + str(self.plot_matplotlib_enabled) + "\n" + + "Hybridpath path following enabled: " + str(self.hybridpath_path_following) + "\n") + + + def send_waypoint_request(self): + req = Waypoint.Request() + req.waypoint = self.waypoints + future = self.waypoint_client.call_async(req) + future.add_done_callback(self.waypoint_response_callback) + + def waypoint_response_callback(self, future): + try: + response = future.result() + self.get_logger().info(f'Received response: {response}') + except Exception as e: + self.get_logger().error(f'Service call failed: {e}') + + def timer_callback(self): + self.simulate_step() + + if self.progress_bar_enabled and not self.sim_finished: + self.show_progressbar() + + if self.num_steps_simulated > self.max_steps and self.plot_matplotlib_enabled and not self.sim_finished: + self.plot_matplotlib() + self.sim_finished = True + + def show_progressbar(self): + progress = (self.num_steps_simulated / self.max_steps) * 100 + filled_length = int(progress // 10) + bar = '[' + '=' * filled_length + ' ' * (10 - filled_length) + ']' + self.get_logger().info(f"Progress: {bar} {progress:.1f}%") + + def guidance_cb(self, msg): + if isinstance(msg, HybridpathReference): + self.x_ref[:3] = np.array([msg.eta_d.x, msg.eta_d.y, msg.eta_d.theta]) + self.yaw_ref_publisher_.publish(Float32(data=self.x_ref[2])) + else: + self.get_logger().error(f"Received message of type {type(msg).__name__}, expected HybridpathReference") + + def wrench_input_cb(self, msg): + self.tau = np.array([msg.force.x, msg.force.y, msg.torque.z]) + tau_values = np.array([[msg.force.x, msg.force.y, msg.torque.z]]) + self.tau_history = np.append(self.tau_history, tau_values, axis=0) + + def update_path_element(self, path, new_pose, N): + if len(path.poses) < N: + path.poses.append(new_pose) + else: + # Shift elements and append new_pose + path.poses.pop(0) + path.poses.append(new_pose) + + def simulate_step(self): + x_next = RK4_integration_step(self.M_inv, self.D, self.state, self.tau, self.dt) + self.state = x_next + + # Pub odom + odometry_msg = state_to_odometrymsg(x_next) + self.state_publisher_.publish(odometry_msg) + + # Pub Posestamped + posestamped_msg = state_to_posestamped(x_next, "world", self.get_clock().now().to_msg()) + self.posestamped_publisher_.publish(posestamped_msg) + + self.yaw_publisher_.publish(Float32(data=x_next[2])) + + # Pub Paths + self.state_path.header.frame_id = "world" + self.state_path.header.stamp = self.get_clock().now().to_msg() + + self.update_path_element(self.state_path, posestamped_msg, self.N) + self.state_path_publisher_.publish(self.state_path) + + xref_msg = state_to_posestamped(self.x_ref, "world", self.get_clock().now().to_msg()) + self.xref_path.header.frame_id = "world" + self.xref_path.header.stamp = self.get_clock().now().to_msg() + + self.update_path_element(self.xref_path, xref_msg, self.N) + self.xref_path_publisher_.publish(self.xref_path) + + # Pub frame and odom tf + self.tf_broadcaster.fixed_frame_broadcaster() + self.tf_broadcaster.handle_pose(x_next) + + # Update current sim step + self.num_steps_simulated += 1 + + def plot_matplotlib(self): + state_x = [pose.pose.position.x for pose in self.state_path.poses] + state_y = [pose.pose.position.y for pose in self.state_path.poses] + ref_x = [pose.pose.position.x for pose in self.xref_path.poses] + ref_y = [pose.pose.position.y for pose in self.xref_path.poses] + + time = np.linspace(0, self.T, len(state_x)) + tau_time = np.linspace(0, self.T, len(self.tau_history)) + + plt.figure() + plt.plot(state_x, state_y, label='State Path') + plt.plot(ref_x, ref_y, label='Reference Path') + plt.title('ASV Path') + plt.xlabel('X') + plt.ylabel('Y') + plt.grid() + plt.legend() + + plt.figure() + plt.plot(time, state_x, label='Actual x') + plt.plot(time, ref_x, label='Reference x') + plt.plot(time, state_y, label='Actual y') + plt.plot(time, ref_y, label='Reference y') + plt.title('Actual position vs reference position') + plt.xlabel('Time [s]') + plt.ylabel('Position [m]') + plt.gca().set_axisbelow(True) + plt.grid() + plt.legend() + + plt.figure() + plt.title('Control input values tau') + plt.subplot(2, 1, 1) + plt.plot(tau_time, self.tau_history[:, 0], label='Surge force') + plt.xlabel('Time [s]') + plt.ylabel('Force [N]') + plt.grid() + plt.legend() + plt.subplot(2, 1, 2) + plt.plot(tau_time, self.tau_history[:, 1], label='Sway force', color='red') + plt.xlabel('Time [s]') + plt.ylabel('Force [N]') + plt.grid() + plt.legend() + + plt.show() + +def main(args=None): + rclpy.init(args=args) + node = ASVSimulatorNode() + rclpy.spin(node) + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/simulation/asv_simulator/asv_simulator/conversions.py b/simulation/asv_simulator/asv_simulator/conversions.py new file mode 100644 index 00000000..92f14cfb --- /dev/null +++ b/simulation/asv_simulator/asv_simulator/conversions.py @@ -0,0 +1,59 @@ +import numpy as np + +from transforms3d.euler import euler2quat, quat2euler +from nav_msgs.msg import Odometry +from geometry_msgs.msg import PoseStamped + + +def odometrymsg_to_state(msg): + x = msg.pose.pose.position.x + y = msg.pose.pose.position.y + orientation_q = msg.pose.pose.orientation + orientation_list = [ + orientation_q.w, orientation_q.x, orientation_q.y, orientation_q.z + ] + + # Convert quaternion to Euler angles, extract yaw + yaw = quat2euler(orientation_list)[2] + + vx = msg.twist.twist.linear.x + vy = msg.twist.twist.linear.y + vyaw = msg.twist.twist.angular.z + + state = np.array([x, y, yaw, vx, vy, vyaw]) + return state + +def state_to_odometrymsg(state): + orientation_list_next = euler2quat(0, 0, state[2]) + + odometry_msg = Odometry() + odometry_msg.pose.pose.position.x = state[0] + odometry_msg.pose.pose.position.y = state[1] + odometry_msg.pose.pose.position.z = 0.0 + odometry_msg.pose.pose.orientation.w = orientation_list_next[0] + odometry_msg.pose.pose.orientation.x = orientation_list_next[1] + odometry_msg.pose.pose.orientation.y = orientation_list_next[2] + odometry_msg.pose.pose.orientation.z = orientation_list_next[3] + + return odometry_msg + +def state_to_posestamped(state, frame_id, stamp): + orientation_list_next = euler2quat(0, 0, state[2]) + + posestamped_msg = PoseStamped() + + posestamped_msg.header.frame_id = frame_id + posestamped_msg.header.stamp = stamp + + posestamped_msg.pose.position.x = state[0] + posestamped_msg.pose.position.y = state[1] + posestamped_msg.pose.position.z = 0.0 + posestamped_msg.pose.orientation.w = orientation_list_next[0] + posestamped_msg.pose.orientation.x = orientation_list_next[1] + posestamped_msg.pose.orientation.y = orientation_list_next[2] + posestamped_msg.pose.orientation.z = orientation_list_next[3] + + return posestamped_msg + +def ssa(angle): + return (angle + np.pi) % (2 * np.pi) - np.pi \ No newline at end of file diff --git a/simulation/asv_simulator/asv_simulator/simulation.py b/simulation/asv_simulator/asv_simulator/simulation.py new file mode 100644 index 00000000..2224d261 --- /dev/null +++ b/simulation/asv_simulator/asv_simulator/simulation.py @@ -0,0 +1,37 @@ +import numpy as np + +def state_dot(M_inv: np.ndarray, D: np.ndarray, state: np.ndarray, tau_actuation: np.ndarray, V_current: np.ndarray = np.zeros(2)) -> np.ndarray: + """ + Calculate the derivative of the state using the non-linear kinematics + """ + heading = state[2] + + J = np.array( + [[np.cos(heading), -np.sin(heading), 0], + [np.sin(heading), np.cos(heading), 0], + [0, 0, 1]] + ) + + A = np.zeros((6,6)) + + A[:3,3:] = J + A[3:, 3:] = - M_inv @ D + + B = np.zeros((6,3)) + B[3:,:] = M_inv + + x_dot = A @ state + B @ tau_actuation + x_dot[0:2] += V_current # add current drift term at velocity level + + return x_dot + +def RK4_integration_step(M_inv: np.ndarray, D: np.ndarray, x: np.ndarray, u: np.ndarray, dt: float) -> np.ndarray: + # integration scheme for simulation, implements the Runge-Kutta 4 integrator + k1 = state_dot(M_inv, D, x, u) + k2 = state_dot(M_inv, D, x+dt/2*k1, u) + k3 = state_dot(M_inv, D, x+dt/2*k2, u) + k4 = state_dot(M_inv, D, x+dt*k3, u) + + x_next = x + dt/6*(k1+2*k2+2*k3+k4) + + return x_next \ No newline at end of file diff --git a/simulation/asv_simulator/config/asv_sim_config.yaml b/simulation/asv_simulator/config/asv_sim_config.yaml new file mode 100644 index 00000000..eb23bd61 --- /dev/null +++ b/simulation/asv_simulator/config/asv_sim_config.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + asv_sim: + plot_matplotlib_enabled: true + progress_bar_enabled: true + hybridpath_path_following: true + T: 200 + waypoints: [0.0, 0.0, 5.0, 5.0, 5.0, 10.0] # [x1, y1, x2, y2, x3, y3] \ No newline at end of file diff --git a/simulation/asv_simulator/config/freya_sim_foxglove_layout.json b/simulation/asv_simulator/config/freya_sim_foxglove_layout.json new file mode 100644 index 00000000..602ac170 --- /dev/null +++ b/simulation/asv_simulator/config/freya_sim_foxglove_layout.json @@ -0,0 +1,214 @@ +{ + "configById": { + "3D!45bpewl": { + "cameraState": { + "perspective": true, + "distance": 250.52738551722237, + "phi": 179.99994266987844, + "thetaOffset": -89.99999770360385, + "targetOffset": [ + 73.97173196727483, + 0.23675590015567488, + 1.2349479935931187e-21 + ], + "target": [ + 0, + 0, + 0 + ], + "targetOrientation": [ + 0, + 0, + 0, + 1 + ], + "fovy": 45, + "near": 0.5, + "far": 5000 + }, + "followMode": "follow-pose", + "followTf": "world", + "scene": { + "transforms": { + "lineWidth": 0.5 + }, + "meshUpAxis": "y_up" + }, + "transforms": {}, + "topics": { + "/sensor/seapath/path/ned": { + "visible": true, + "lineWidth": 0.5, + "gradient": [ + "#ffffffff", + "#00be1080" + ] + }, + "/sensor/seapath/posestamped/ned": { + "visible": true, + "axisScale": 10.2, + "type": "axis", + "arrowScale": [ + 10, + 3.15, + 3.15 + ] + }, + "/move_base_simple/goal": { + "visible": false + }, + "/sensor/seapath/state_path/ned": { + "visible": true, + "lineWidth": 1, + "gradient": [ + "#fafafaff", + "#00ff0d80" + ] + }, + "/sensor/seapath/xref_path/ned": { + "visible": true, + "gradient": [ + "#c750cfff", + "#ae19a9ff" + ], + "lineWidth": 0.5 + } + }, + "layers": { + "6bba1d8c-b741-42b0-901c-20e59bfe5fcc": { + "visible": true, + "frameLocked": true, + "label": "Grid", + "instanceId": "6bba1d8c-b741-42b0-901c-20e59bfe5fcc", + "layerId": "foxglove.Grid", + "size": 1000, + "divisions": 100, + "color": "#248eff", + "position": [ + 0, + 0, + 0 + ], + "rotation": [ + 0, + 0, + 0 + ], + "order": 1, + "lineWidth": 0.3 + } + }, + "publish": { + "type": "point", + "poseTopic": "/move_base_simple/goal", + "pointTopic": "/clicked_point", + "poseEstimateTopic": "/initialpose", + "poseEstimateXDeviation": 0.5, + "poseEstimateYDeviation": 0.5, + "poseEstimateThetaDeviation": 0.26179939 + }, + "imageMode": {} + }, + "Plot!41trey": { + "paths": [ + { + "timestampMethod": "receiveTime", + "value": "/sensor/seapath/posestamped/ned.pose.position.x", + "enabled": true + }, + { + "timestampMethod": "receiveTime", + "value": "/controller/lqr/reference.pose.pose.position.x", + "enabled": true + } + ], + "showXAxisLabels": true, + "showYAxisLabels": true, + "showLegend": false, + "legendDisplay": "floating", + "showPlotValuesInLegend": true, + "isSynced": true, + "xAxisVal": "timestamp", + "sidebarDimension": 240, + "followingViewWidth": 30, + "foxglovePanelTitle": "Surge" + }, + "Plot!q6f7ui": { + "paths": [ + { + "timestampMethod": "receiveTime", + "value": "/sensor/seapath/posestamped/ned.pose.position.y", + "enabled": true + }, + { + "timestampMethod": "receiveTime", + "value": "/controller/lqr/reference.pose.pose.position.y", + "enabled": true + } + ], + "showXAxisLabels": true, + "showYAxisLabels": true, + "showLegend": false, + "legendDisplay": "floating", + "showPlotValuesInLegend": true, + "isSynced": true, + "xAxisVal": "timestamp", + "sidebarDimension": 240, + "followingViewWidth": 30, + "foxglovePanelTitle": "Sway" + }, + "Tab!23gzpt8": { + "activeTabIdx": 0, + "tabs": [ + { + "title": "1", + "layout": { + "first": "3D!45bpewl", + "second": { + "first": "Plot!41trey", + "second": { + "first": "Plot!q6f7ui", + "second": "Plot!4ia1dcm", + "direction": "row" + }, + "direction": "row", + "splitPercentage": 29.72972972972973 + }, + "direction": "column", + "splitPercentage": 69.86721144024514 + } + } + ] + }, + "Plot!4ia1dcm": { + "paths": [ + { + "timestampMethod": "receiveTime", + "value": "/sensor/seapath/yaw/ned.data", + "enabled": true + }, + { + "timestampMethod": "receiveTime", + "value": "/sensor/seapath/yaw_ref/ned.data", + "enabled": true + } + ], + "showXAxisLabels": true, + "showYAxisLabels": true, + "showLegend": true, + "legendDisplay": "floating", + "showPlotValuesInLegend": true, + "isSynced": true, + "xAxisVal": "timestamp", + "sidebarDimension": 240, + "followingViewWidth": 30, + "foxglovePanelTitle": "Yaw" + } + }, + "globalVariables": {}, + "userNodes": {}, + "playbackConfig": { + "speed": 1 + }, + "layout": "Tab!23gzpt8" +} \ No newline at end of file diff --git a/simulation/asv_simulator/launch/asv_simulator.launch.py b/simulation/asv_simulator/launch/asv_simulator.launch.py new file mode 100644 index 00000000..77cbe70a --- /dev/null +++ b/simulation/asv_simulator/launch/asv_simulator.launch.py @@ -0,0 +1,20 @@ +import os +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + asv_simulator_node = Node( + package='asv_simulator', + executable='asv_simulator_node.py', + name='asv_simulator_node', + output='screen', + parameters=[ + os.path.join(get_package_share_directory('asv_setup'), 'config', 'robots', 'freya.yaml'), + os.path.join(get_package_share_directory('asv_simulator'), 'config', 'asv_sim_config.yaml')] + ) + + return LaunchDescription([ + asv_simulator_node + ]) diff --git a/simulation/asv_simulator/package.xml b/simulation/asv_simulator/package.xml new file mode 100644 index 00000000..52befdf9 --- /dev/null +++ b/simulation/asv_simulator/package.xml @@ -0,0 +1,28 @@ + + + + asv_simulator + 0.0.0 + This package provides the implementation of the asv_simulator for the Vortex ASV. + vortex + MIT + + ament_cmake_python + + rclpy + python-transforms3d-pip + nav_msgs + geometry_msgs + vortex_msgs + std_msgs + numpy + matplotlib + scipy + + python3-pytest + + + + ament_cmake + + \ No newline at end of file