diff --git a/guidance/dp_guidance/CMakeLists.txt b/guidance/dp_guidance/CMakeLists.txt new file mode 100644 index 00000000..7cc58d78 --- /dev/null +++ b/guidance/dp_guidance/CMakeLists.txt @@ -0,0 +1,27 @@ +cmake_minimum_required(VERSION 3.8) +project(dp_guidance) + +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 + dp_guidance/dp_guidance_node.py + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() \ No newline at end of file diff --git a/guidance/dp_guidance/README.md b/guidance/dp_guidance/README.md new file mode 100755 index 00000000..3dbdad76 --- /dev/null +++ b/guidance/dp_guidance/README.md @@ -0,0 +1,15 @@ +# Dynamic Positioning (DP) Guidance + +This package provides the implementation of DP guidance for the Vortex ASV. + +## Usage + +To use the DP guidance launch it using: `ros2 launch dp_guidance dp_guidance.launch` + +To run with custom waypoints (replace example waypoints with actual waypoints, and add as many prefered): + +`ros2 service call waypoint_list vortex_msgs/srv/Waypoint "waypoint: [{x: 0.0, y: 0.0, z: 0.0}, {x: 5.0, y: 5.0, z: 0.0}]"` + +## Configuration + +You can configure the behavior of the hybrid path guidance by modifying the parameters in the `config` directory. \ No newline at end of file diff --git a/guidance/dp_guidance/config/dp_guidance_config.yaml b/guidance/dp_guidance/config/dp_guidance_config.yaml new file mode 100644 index 00000000..02db4e76 --- /dev/null +++ b/guidance/dp_guidance/config/dp_guidance_config.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + dp_guidance: + dt: 0.1 # Time step \ No newline at end of file diff --git a/guidance/dp_guidance/dp_guidance/__init__.py b/guidance/dp_guidance/dp_guidance/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/guidance/dp_guidance/dp_guidance/conversions.py b/guidance/dp_guidance/dp_guidance/conversions.py new file mode 100644 index 00000000..92f14cfb --- /dev/null +++ b/guidance/dp_guidance/dp_guidance/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/guidance/dp_guidance/dp_guidance/dp_guidance_node.py b/guidance/dp_guidance/dp_guidance/dp_guidance_node.py new file mode 100755 index 00000000..a416f952 --- /dev/null +++ b/guidance/dp_guidance/dp_guidance/dp_guidance_node.py @@ -0,0 +1,88 @@ +#!/usr/bin/env python3 + +import numpy as np +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import Pose2D +from vortex_msgs.srv import Waypoint +from nav_msgs.msg import Odometry +from transforms3d.euler import quat2euler + +from dp_guidance.conversions import odometrymsg_to_state, state_to_odometrymsg +from dp_guidance.reference_filter import ReferenceFilter + +class Guidance(Node): + def __init__(self): + super().__init__("dp_guidance") + self.declare_parameters( + namespace='', + parameters=[ + ('dp_guidance.dt', 0.1) + ]) + + self.waypoint_server = self.create_service(Waypoint, 'waypoint_list', self.waypoint_callback) + self.eta_subscriber_ = self.state_subscriber_ = self.create_subscription(Odometry, '/sensor/seapath/odom/ned', self.eta_callback, 1) + self.guidance_publisher = self.create_publisher(Odometry, 'guidance/dp/reference', 1) + + # Get parameters + self.dt = self.get_parameter('dp_guidance.dt').get_parameter_value().double_value + + # Flags for logging + self.waypoints_received = False + self.waiting_message_printed = False + + self.init_pos = False + + self.eta = np.array([0, 0, 0]) + self.eta_ref = np.array([0, 0, 0]) + + self.xd = np.zeros(9) + + self.reference_filter = ReferenceFilter() + + # Timer for guidance + timer_period = 0.1 + self.timer = self.create_timer(timer_period, self.guidance_callback) + + def waypoint_callback(self, request, response): + self.waypoints = request.waypoint + self.get_logger().info(f'Received waypoints: {self.waypoints}') + self.waypoints_received = True + self.waiting_message_printed = False # Reset this flag to handle multiple waypoint sets + response.success = True + return response + + def eta_callback(self, msg: Odometry): + self.eta = odometrymsg_to_state(msg)[:3] + + def guidance_callback(self): + if self.waypoints_received: + if not self.init_pos: + self.xd[0:3] = self.eta + self.init_pos = True + last_waypoint = self.waypoints[-1] + self.eta_ref = np.array([last_waypoint.x, last_waypoint.y, 0]) + x_next = self.reference_filter.step(self.eta_ref, self.xd) + self.xd = x_next + self.get_logger().info(f'x_next[0]: {x_next[0]}') + self.get_logger().info(f'x_next[0]: {x_next[1]}') + self.get_logger().info(f'x_next[0]: {x_next[2]}') + + odom_msg = Odometry() + odom_msg = state_to_odometrymsg(x_next[:3]) + self.guidance_publisher.publish(odom_msg) + + else: + if not self.waiting_message_printed: + self.get_logger().info('Waiting for waypoints to be received') + self.waiting_message_printed = True + +def main(args=None): + rclpy.init(args=args) + guidance = Guidance() + rclpy.spin(guidance) + guidance.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/guidance/dp_guidance/dp_guidance/reference_filter.py b/guidance/dp_guidance/dp_guidance/reference_filter.py new file mode 100644 index 00000000..1ad4f344 --- /dev/null +++ b/guidance/dp_guidance/dp_guidance/reference_filter.py @@ -0,0 +1,72 @@ +#!/usr/bin/env python3 + +import numpy as np +import control +import matplotlib.pyplot as plt + +class ReferenceFilter: + def __init__(self): + zeta = 0.8 + omega_b = 0.05 + omega_n = omega_b/np.sqrt(1-2*zeta**2 + np.sqrt(4*zeta**4 - 4*zeta**2 + 2)) + + I = np.eye(3) + Omega = 2*zeta*omega_n*I + # Gamma = omega_n**2*I + Delta = zeta*I + Ad = np.zeros((9,9)) + Ad[0:3,3:6] = I + Ad[3:6,6:9] = I + Ad[6:9,0:3] = -Omega**3 + Ad[6:9,3:6] = -(2*Delta+I)@Omega**2 + Ad[6:9,6:9] = -(2*Delta+I)@Omega + + Bd = np.zeros((9,3)) + Bd[6:9,:] = Omega**3 + + sys = control.ss(Ad, Bd, np.zeros((9,9)), np.zeros((9,3))) + sysd = control.c2d(sys, 0.1) + + self.Ad = sysd.A + self.Bd = sysd.B + + def step(self, r, xd): + x_next = self.Ad@xd + self.Bd@r + return x_next + + def get_eta(self, xd): + return xd[:,0:3] + + def get_nu(self, xd): + nu = np.zeros((len(xd),3)) + for i in range(len(xd)): + psi = xd[i,2] + nu[i,:] = (self.rotationMatrix(psi).transpose())@xd[i,3:6] + return nu + + @staticmethod + def rotationMatrix(psi): + R = np.array([[np.cos(psi), -np.sin(psi), 0], + [np.sin(psi), np.cos(psi), 0], + [0, 0, 1]]) + return R + + +if __name__ == "__main__": + refFilter = ReferenceFilter() + ref_pos = np.array([5, 10, 0]) + t = np.arange(0, 100, 0.1) + N = len(t) + x_d = np.zeros((N, 9)) + + for k in range(1,N): + x_d[k,:] = refFilter.step(ref_pos, x_d[k-1, :]) + + eta_d = refFilter.get_eta(x_d) + nu_d = refFilter.get_nu(x_d) + plt.figure() + plt.plot(t, eta_d[:,0]) + + plt.figure() + plt.plot(t, nu_d[:,0]) + plt.show() \ No newline at end of file diff --git a/guidance/dp_guidance/launch/dp_guidance.launch.py b/guidance/dp_guidance/launch/dp_guidance.launch.py new file mode 100644 index 00000000..710ed9e0 --- /dev/null +++ b/guidance/dp_guidance/launch/dp_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(): + dp_guidance_node = Node( + package='dp_guidance', + executable='dp_guidance_node.py', + name='dp_guidance_node', + parameters=[os.path.join(get_package_share_directory('dp_guidance'),'config','dp_guidance_config.yaml')], + output='screen', + ) + return LaunchDescription([ + dp_guidance_node + ]) diff --git a/guidance/dp_guidance/package.xml b/guidance/dp_guidance/package.xml new file mode 100644 index 00000000..33547bcb --- /dev/null +++ b/guidance/dp_guidance/package.xml @@ -0,0 +1,21 @@ + + + + dp_guidance + 0.0.0 + This package provides the implementation of hybrid path guidance for the Vortex ASV. + vortex + MIT + + rclpy + nav_msgs + python-transforms3d-pip + geometry_msgs + vortex_msgs + + python3-pytest + + + ament_cmake + + diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py index f319dc60..6fd881e7 100755 --- a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py +++ b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py @@ -65,6 +65,7 @@ def waypoint_callback(self, request, response): return response def switching_callback(self, msg: Int32): + print(msg.data) self.switching_waypoint = msg.data def eta_callback(self, msg: Odometry): @@ -100,7 +101,7 @@ def guidance_callback(self): self.waiting_message_printed = False self.get_logger().info('Last waypoint reached') #self.switching_publisher.publish(True) - if self.s >= self.path.NumSubpaths-1: + if self.s >= self.path.NumSubpaths-self.switching_waypoint: self.switching_publisher.publish(Bool(data=True)) else: diff --git a/motion/hybridpath_controller/config/hybridpath_controller_config.yaml b/motion/hybridpath_controller/config/hybridpath_controller_config.yaml index 7deeec2f..f3a37dd6 100644 --- a/motion/hybridpath_controller/config/hybridpath_controller_config.yaml +++ b/motion/hybridpath_controller/config/hybridpath_controller_config.yaml @@ -1,5 +1,6 @@ /**: ros__parameters: hybridpath_controller: - K1_diag: [1.0, 1.0, 1.0] # First gain matrix + K1_diag: [1.5, 0.9, 1.0] # First gain matrix + #K2_diag: [10.0, 10.0, 5.0] # Second gain matrix K2_diag: [60.0, 59.0, 47.0] # Second gain matrix \ No newline at end of file diff --git a/simulation/asv_simulator/asv_simulator/asv_simulator_node.py b/simulation/asv_simulator/asv_simulator/asv_simulator_node.py index e9c4a5b4..266c48de 100644 --- a/simulation/asv_simulator/asv_simulator/asv_simulator_node.py +++ b/simulation/asv_simulator/asv_simulator/asv_simulator_node.py @@ -189,6 +189,8 @@ def plot_matplotlib(self): 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] + waypoints_x = [point.x for point in self.waypoints] + waypoints_y = [point.y for point in self.waypoints] time = np.linspace(0, self.T, len(state_x)) tau_time = np.linspace(0, self.T, len(self.tau_history)) @@ -196,6 +198,7 @@ def plot_matplotlib(self): plt.figure() plt.plot(state_x, state_y, label='State Path') plt.plot(ref_x, ref_y, label='Reference Path') + plt.scatter(waypoints_x, waypoints_y, color='red', label='Waypoints') # Plotting waypoints in red plt.title('ASV Path') plt.xlabel('X') plt.ylabel('Y')