diff --git a/guidance/dp_guidance/dp_guidance/dp_guidance_node.py b/guidance/dp_guidance/dp_guidance/dp_guidance_node.py index a416f952..173d0fa4 100755 --- a/guidance/dp_guidance/dp_guidance/dp_guidance_node.py +++ b/guidance/dp_guidance/dp_guidance/dp_guidance_node.py @@ -7,6 +7,7 @@ from vortex_msgs.srv import Waypoint from nav_msgs.msg import Odometry from transforms3d.euler import quat2euler +from std_msgs.msg import String from dp_guidance.conversions import odometrymsg_to_state, state_to_odometrymsg from dp_guidance.reference_filter import ReferenceFilter @@ -21,8 +22,10 @@ def __init__(self): ]) 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.eta_subscriber_ = self.create_subscription(Odometry, '/sensor/seapath/odom/ned', self.eta_callback, 1) self.guidance_publisher = self.create_publisher(Odometry, 'guidance/dp/reference', 1) + self.active_controller_subscriber = self.create_subscription(String, 'mission/controller', self.active_controller_callback, 10) + self.active_controller = False # Get parameters self.dt = self.get_parameter('dp_guidance.dt').get_parameter_value().double_value @@ -52,14 +55,20 @@ def waypoint_callback(self, request, response): response.success = True return response + def active_controller_callback(self, msg: String): + if msg.data == 'DP': + self.active_controller = True + else: + self.active_controller = False + 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 + if self.waypoints_received and self.active_controller: + #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) @@ -67,6 +76,7 @@ def guidance_callback(self): 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]}') + self.get_logger().info(f'eta_ref : {self.eta_ref}') odom_msg = Odometry() odom_msg = state_to_odometrymsg(x_next[:3]) @@ -74,7 +84,7 @@ def guidance_callback(self): else: if not self.waiting_message_printed: - self.get_logger().info('Waiting for waypoints to be received') + self.get_logger().info('Waiting for controller switching') self.waiting_message_printed = True def main(args=None): diff --git a/guidance/dp_guidance/dp_guidance/reference_filter.py b/guidance/dp_guidance/dp_guidance/reference_filter.py index 1ad4f344..5d59c9eb 100644 --- a/guidance/dp_guidance/dp_guidance/reference_filter.py +++ b/guidance/dp_guidance/dp_guidance/reference_filter.py @@ -7,7 +7,7 @@ class ReferenceFilter: def __init__(self): zeta = 0.8 - omega_b = 0.05 + omega_b = 0.5 omega_n = omega_b/np.sqrt(1-2*zeta**2 + np.sqrt(4*zeta**4 - 4*zeta**2 + 2)) I = np.eye(3) diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath.py b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath.py index 68790b67..a9c34e8d 100644 --- a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath.py +++ b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath.py @@ -67,7 +67,7 @@ class HybridPathGenerator: """ def __init__(self, WP: list[Point], r: int, lambda_val: float): # Convert the waypoints to a numpy array - WP_arr = np.array([[int(wp.x), int(wp.y)] for wp in WP]) + WP_arr = np.array([[wp.x, wp.y] for wp in WP]) if len(WP_arr) == 2: # The generator must have at least 3 waypoints to work self.WP = np.array([WP_arr[0], [(WP_arr[0][0] + WP_arr[1][0])/2, (WP_arr[0][1] + WP_arr[1][1])/2], WP_arr[1]]) @@ -296,7 +296,7 @@ def update_s(path: Path, dt: float, u_desired: float, s: float, w: float) -> flo """ signals = HybridPathSignals(path, s) - v_s = signals.get_vs() + v_s = signals.get_vs(u_desired) s_new = s + (v_s + w) * dt return s_new @@ -314,12 +314,11 @@ class HybridPathSignals: Path (Path): The path object. s (float): The path parameter. """ - def __init__(self, path: Path, s: float, u_desired: float = 0.5): + def __init__(self, path: Path, s: float): if not isinstance(path, Path): raise TypeError("path must be an instance of Path") self.path = path self.s = self._clamp_s(s, self.path.NumSubpaths) - self.u_desired = u_desired def _clamp_s(self, s: float, num_subpaths: int) -> float: """ @@ -445,7 +444,7 @@ def get_heading_second_derivative(self) -> float: psi_dder = (p_der[0] * p_ddder[1] - p_der[1] * p_ddder[0]) / (p_der[0]**2 + p_der[1]**2) - 2 * (p_der[0] * p_dder[1] - p_dder[0] * p_der[1]) * (p_der[0] * p_dder[0] - p_dder[1] * p_der[0]) / ((p_der[0]**2 + p_der[1]**2)**2) return psi_dder - def get_vs(self) -> float: + def get_vs(self, u_desired) -> float: """ Calculate the reference velocity. @@ -459,10 +458,10 @@ def get_vs(self) -> float: p_der = self.get_derivatives()[0] # Calculate the reference velocity - v_s = self.u_desired / np.linalg.norm(p_der) + v_s = u_desired / np.linalg.norm(p_der) return v_s - def get_vs_derivative(self) -> float: + def get_vs_derivative(self, u_desired) -> float: """ Calculate the derivative of the reference velocity. @@ -477,7 +476,7 @@ def get_vs_derivative(self) -> float: p_dder = self.get_derivatives()[1] # Calculate the derivative of the reference velocity - v_s_s = -self.u_desired * (np.dot(p_der, p_dder)) / (np.sqrt(p_der[0]**2 + p_der[1]**2)**3) + v_s_s = -u_desired * (np.dot(p_der, p_dder)) / (np.sqrt(p_der[0]**2 + p_der[1]**2)**3) return v_s_s def get_w(self, mu: float, eta: np.ndarray) -> float: diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py index 6fd881e7..77b14225 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, Int32 +from std_msgs.msg import Bool, Float32 from hybridpath_guidance.hybridpath import HybridPathGenerator, HybridPathSignals @@ -27,7 +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) + self.switching_criteria_subscriber = self.create_subscription(Float32, 'guidance/hybridpath/switching', self.switching_callback, 10) # Get parameters self.lambda_val = self.get_parameter('hybridpath_guidance.lambda_val').get_parameter_value().double_value @@ -36,7 +36,9 @@ 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 + self.switching_waypoint = 1.0 + + self.u_desired = 0.25 # Desired velocity # Flags for logging self.waypoints_received = False @@ -58,13 +60,12 @@ def waypoint_callback(self, request, response): self.s = 0 signals = HybridPathSignals(self.path, self.s) - self.u_desired = signals.u_desired self.w = signals.get_w(self.mu, self.eta) response.success = True return response - def switching_callback(self, msg: Int32): + def switching_callback(self, msg: Float32): print(msg.data) self.switching_waypoint = msg.data @@ -91,8 +92,8 @@ def guidance_callback(self): hp_msg.eta_d_ss = Pose2D(x=pos_dder[0], y=pos_dder[1], theta=psi_dder) hp_msg.w = signals.get_w(self.mu, self.eta) - hp_msg.v_s = signals.get_vs() - hp_msg.v_ss = signals.get_vs_derivative() + hp_msg.v_s = signals.get_vs(self.u_desired) + hp_msg.v_ss = signals.get_vs_derivative(self.u_desired) self.guidance_publisher.publish(hp_msg) diff --git a/mission/mission_planner/mission_planner/mission_planner.py b/mission/mission_planner/mission_planner/mission_planner.py index 15c6ff8b..e0dcf06d 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, Int32 +from std_msgs.msg import String, Bool, Float32 class MissionPlannerClient(Node): """ @@ -30,7 +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) + self.switching_waypoint_publisher = self.create_publisher(Float32, 'guidance/hybridpath/switching', 10) def timer_callback(self): msg = String() @@ -39,8 +39,8 @@ def timer_callback(self): self.get_logger().info(f'Publishing: {self.active_controller}') # Switching criteria - msg = Int32() - msg.data = 1 + msg = Float32() + msg.data = 1.0 self.switching_waypoint_publisher.publish(msg) def guidance_callback(self, msg): diff --git a/motion/lqr_controller/config/lqr_config.yaml b/motion/lqr_controller/config/lqr_config.yaml index ac497221..81bf35bb 100644 --- a/motion/lqr_controller/config/lqr_config.yaml +++ b/motion/lqr_controller/config/lqr_config.yaml @@ -1,5 +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] + Q: [20.0, 20.0, 1.0, 1.0, 1.0, 1.0] # State costs for [x, y, heading, x_dot, y_dot, heading_dot] R: [1.0, 1.0, 1.0] # Control cost weight \ 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 index 91e8ac28..58ba17a2 100644 --- a/motion/lqr_controller/lqr_controller/lqr_controller_node.py +++ b/motion/lqr_controller/lqr_controller/lqr_controller_node.py @@ -47,7 +47,7 @@ def __init__(self): self.LQR = LQRController(M, D, Q, R, heading_ref) - self.x_ref = [5, 10, 0] + self.x_ref = [0, 0, 0] self.state = [0, 0, 0, 0, 0, 0] self.enabled = False diff --git a/motion/pid_controller/CMakeLists.txt b/motion/pid_controller/CMakeLists.txt new file mode 100644 index 00000000..0c45a92b --- /dev/null +++ b/motion/pid_controller/CMakeLists.txt @@ -0,0 +1,27 @@ +cmake_minimum_required(VERSION 3.8) +project(pid_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 +pid_controller/pid_controller_node.py + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() \ No newline at end of file diff --git a/motion/pid_controller/config/pid_config.yaml b/motion/pid_controller/config/pid_config.yaml new file mode 100644 index 00000000..f04befd2 --- /dev/null +++ b/motion/pid_controller/config/pid_config.yaml @@ -0,0 +1,6 @@ +/**: + ros__parameters: + pid_controller: + Kp: [23.04, 34.56, 4.032] # Proportional costs for [x, y, heading] + Ki: [2.76, 4.1, 0.48] # Integral costs for [x, y, heading] + Kd: [28.7648, 43.1472, 5.048384] # Derivative costs for [x, y, heading] \ No newline at end of file diff --git a/motion/pid_controller/launch/pid_controller.launch.py b/motion/pid_controller/launch/pid_controller.launch.py new file mode 100644 index 00000000..465924a9 --- /dev/null +++ b/motion/pid_controller/launch/pid_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(): + pid_controller_node = Node( + package='pid_controller', + executable='pid_controller_node.py', + name='pid_controller_node', + parameters=[ + os.path.join(get_package_share_directory('pid_controller'),'config','pid_config.yaml'), + os.path.join(get_package_share_directory('asv_setup'), 'config', 'robots', 'freya.yaml') + ], + output='screen', + ) + return LaunchDescription([ + pid_controller_node + ]) diff --git a/motion/pid_controller/package.xml b/motion/pid_controller/package.xml new file mode 100644 index 00000000..1cb6be76 --- /dev/null +++ b/motion/pid_controller/package.xml @@ -0,0 +1,27 @@ + + + + pid_controller + 0.0.0 + This is an implementation of the PID 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/motion/pid_controller/pid_controller/__init__.py b/motion/pid_controller/pid_controller/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/motion/pid_controller/pid_controller/conversions.py b/motion/pid_controller/pid_controller/conversions.py new file mode 100644 index 00000000..92f14cfb --- /dev/null +++ b/motion/pid_controller/pid_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/pid_controller/pid_controller/pid_controller.py b/motion/pid_controller/pid_controller/pid_controller.py new file mode 100644 index 00000000..1207fbb1 --- /dev/null +++ b/motion/pid_controller/pid_controller/pid_controller.py @@ -0,0 +1,43 @@ +#!/usr/bin/env python3 + +import numpy as np + +class PID: + def __init__(self, Kp, Ki, Kd): + self.error_sum = np.zeros(3) + + self.Kp = np.diag(Kp) + self.Ki = np.diag(Ki) + self.Kd = np.diag(Kd) + + self.tau_max = np.array([100, 100, 100]) + + def calculate_control_input(self, eta, eta_d, eta_dot, dt): + error = eta - eta_d + self.error_sum += error * dt + self.error_sum = np.clip(self.error_sum, -20, 20) + + p = self.Kp @ error + i = 0 #self.Ki @ self.error_sum + d = self.Kd @ eta_dot + + self.last_error = error + + tau = -(p + i + d) + + if tau[0] > self.tau_max[0]: + tau[0] = self.tau_max[0] + elif tau[0] < -self.tau_max[0]: + tau[0] = -self.tau_max[0] + + if tau[1] > self.tau_max[1]: + tau[1] = self.tau_max[1] + elif tau[1] < -self.tau_max[1]: + tau[1] = -self.tau_max[1] + + if tau[2] > self.tau_max[2]: + tau[2] = self.tau_max[2] + elif tau[2] < -self.tau_max[2]: + tau[2] = -self.tau_max[2] + + return tau \ No newline at end of file diff --git a/motion/pid_controller/pid_controller/pid_controller_node.py b/motion/pid_controller/pid_controller/pid_controller_node.py new file mode 100644 index 00000000..70ab1d30 --- /dev/null +++ b/motion/pid_controller/pid_controller/pid_controller_node.py @@ -0,0 +1,95 @@ +#!/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 pid_controller.pid_controller import PID +from pid_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 PIDControllerNode(Node): + def __init__(self): + super().__init__("pid_controller_node") + + self.declare_parameters( + namespace='', + parameters=[ + ('pid_controller.Kp', [1.0, 1.0, 1.0]), + ('pid_controller.Ki', [1.0, 1.0, 1.0]), + ('pid_controller.Kd', [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]) + ]) + + 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 + + Kp = self.get_parameter('pid_controller.Kp').get_parameter_value().double_array_value + Ki = self.get_parameter('pid_controller.Ki').get_parameter_value().double_array_value + Kd = self.get_parameter('pid_controller.Kd').get_parameter_value().double_array_value + M = self.get_parameter('physical.inertia_matrix').get_parameter_value().double_array_value + + M = np.reshape(M, (3, 3)) + M_diag = np.diag(M) + + ## PID TUNING VALUES ## (OVERWRITES YAML FILE VALUES) + omega_n = 1.2 + zeta = 0.75 + Kp = M_diag * omega_n**2 + Kd = M_diag * 2 * zeta * omega_n #- D_diag + Ki = omega_n/10 * Kp + + self.pid = PID(Kp, Ki, Kd) + + self.x_ref = np.array([0, 0, 0]) + self.state = np.array([0, 0, 0, 0, 0, 0]) + + self.enabled = False + + self.controller_period = 0.1 + self.controller_timer_ = self.create_timer(self.controller_period, self.controller_callback) + + self.get_logger().info("pid_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: + control_input = self.pid.calculate_control_input(self.state[:3], self.x_ref, self.state[3:], self.controller_period) + self.get_logger().info(f"Control input: {control_input}") + 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 = PIDControllerNode() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + +if __name__ == "__main__": + main() diff --git a/simulation/asv_simulator/config/asv_sim_config.yaml b/simulation/asv_simulator/config/asv_sim_config.yaml index eb23bd61..e6f92889 100644 --- a/simulation/asv_simulator/config/asv_sim_config.yaml +++ b/simulation/asv_simulator/config/asv_sim_config.yaml @@ -4,5 +4,5 @@ plot_matplotlib_enabled: true progress_bar_enabled: true hybridpath_path_following: true - T: 200 + T: 300 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