From 7050c3b75037a93e375becedf8734ebfb5bde201 Mon Sep 17 00:00:00 2001 From: Andeshog <157955984+Andeshog@users.noreply.github.com> Date: Tue, 9 Jul 2024 19:42:57 +0200 Subject: [PATCH] . --- .../dp_guidance/dp_guidance_node.py | 9 +- guidance/hybridpath_guidance/CMakeLists.txt | 1 + .../config/hybridpath_guidance_config.yaml | 2 +- .../hybridpath_guidance/hybridpath.py | 89 +++++++++-- .../hybridpath_guidance_node.py | 146 +++++++++++++----- .../noisy_odom_publisher.py | 57 +++++++ .../hybridpath_guidance/u_desired_node.py | 48 ++++++ .../hybridpath_guidance/waypoint_node.py | 2 +- .../config/hybridpath_controller_config.yaml | 4 +- .../adaptive_backstep.py | 39 ++++- .../hybridpath_controller_node.py | 84 ++++++++-- 11 files changed, 407 insertions(+), 74 deletions(-) create mode 100755 guidance/hybridpath_guidance/hybridpath_guidance/noisy_odom_publisher.py create mode 100644 guidance/hybridpath_guidance/hybridpath_guidance/u_desired_node.py diff --git a/guidance/dp_guidance/dp_guidance/dp_guidance_node.py b/guidance/dp_guidance/dp_guidance/dp_guidance_node.py index b329a483..483b5074 100755 --- a/guidance/dp_guidance/dp_guidance/dp_guidance_node.py +++ b/guidance/dp_guidance/dp_guidance/dp_guidance_node.py @@ -37,6 +37,7 @@ def __init__(self): self.init_pos = False self.eta_received = False + self.eta_logger = True self.eta = np.array([0, 0, 0]) self.eta_ref = np.array([0, 0, 0]) @@ -68,7 +69,7 @@ def guidance_callback(self): self.get_logger().info(f"Reference initialized at {self.xd[0:3]}") self.init_pos = True last_waypoint = self.waypoints[-1] - self.eta_ref = np.array([last_waypoint.x, last_waypoint.y, 0]) + self.eta_ref = np.array([last_waypoint.x, last_waypoint.y, self.eta[2]]) 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]}') @@ -76,12 +77,14 @@ def guidance_callback(self): # self.get_logger().info(f'x_next[0]: {x_next[2]}') odom_msg = Odometry() - odom_msg = state_to_odometrymsg(x_next[:3]) + # odom_msg = state_to_odometrymsg(x_next[:3]) + odom_msg = state_to_odometrymsg(self.eta_ref) self.guidance_publisher.publish(odom_msg) else: - if not self.eta_received: + if not self.eta_received and self.eta_logger: self.get_logger().info("Waiting for eta") + self.eta_logger = False if not self.waiting_message_printed: self.get_logger().info('Waiting for waypoints to be received') diff --git a/guidance/hybridpath_guidance/CMakeLists.txt b/guidance/hybridpath_guidance/CMakeLists.txt index e16c1b10..8b4fd5ac 100644 --- a/guidance/hybridpath_guidance/CMakeLists.txt +++ b/guidance/hybridpath_guidance/CMakeLists.txt @@ -22,6 +22,7 @@ install(DIRECTORY install(PROGRAMS hybridpath_guidance/hybridpath_guidance_node.py hybridpath_guidance/waypoint_node.py + hybridpath_guidance/noisy_odom_publisher.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/guidance/hybridpath_guidance/config/hybridpath_guidance_config.yaml b/guidance/hybridpath_guidance/config/hybridpath_guidance_config.yaml index 20fa168f..7279c745 100644 --- a/guidance/hybridpath_guidance/config/hybridpath_guidance_config.yaml +++ b/guidance/hybridpath_guidance/config/hybridpath_guidance_config.yaml @@ -4,4 +4,4 @@ lambda_val: 0.15 # Curvature constant path_generator_order: 1 # Differentiability order dt: 0.1 # Time step - mu: 0.15 # Tuning parameter, changes how much the path will wait for the vessel if it lags behind \ No newline at end of file + mu: 0.25 # Tuning parameter, changes how much the path will wait for the vessel if it lags behind \ No newline at end of file diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath.py b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath.py index 9f2a6155..a4a4db3c 100644 --- a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath.py +++ b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath.py @@ -66,6 +66,40 @@ class HybridPathGenerator: path (Path): The path object. """ def __init__(self, WP: list[Point], r: int, lambda_val: float): + self.WP = WP + self.r = r + self.lambda_val = lambda_val + self.order = 2*r + 1 + + def create_path(self, WP: list[Point]) -> None: + """ + Create a path object. + + Args: + WP (list[Point]): A list of waypoints. + + Returns: + None + """ + self.update_waypoints(WP) + self._initialize_path() + self._calculate_subpaths() + + def _initialize_path(self): + self.path = Path() + + self.path.coeff.a = [] + self.path.coeff.b = [] + self.path.coeff.a_der = [] + self.path.coeff.b_der = [] + self.path.LinSys.A = None + self.path.LinSys.bx = [] + self.path.LinSys.by = [] + + self.path.NumSubpaths = len(self.WP) - 1 + self.path.Order = self.order + + def update_waypoints(self, WP: list[Point]) -> None: # Convert the waypoints to a numpy array WP_arr = np.array([[wp.x, wp.y] for wp in WP]) @@ -77,13 +111,6 @@ def __init__(self, WP: list[Point], r: int, lambda_val: float): else: self.WP = WP_arr - self.r = r - self.lambda_val = lambda_val - self.order = 2*r + 1 - self.path = Path() - self.path.NumSubpaths = len(self.WP) - 1 - self.path.Order = self.order - self._calculate_subpaths() def _construct_A_matrix(self) -> np.ndarray: """ @@ -280,6 +307,12 @@ def _calculate_subpaths(self) -> None: for k, (a, b) in enumerate(zip(a_derivatives, b_derivatives)): self._append_derivatives(k, a, b) + def get_path(self) -> Path: + """ + Get the hybrid path. + """ + return self.path + @staticmethod def update_s(path: Path, dt: float, u_desired: float, s: float, w: float) -> float: """ @@ -295,7 +328,9 @@ def update_s(path: Path, dt: float, u_desired: float, s: float, w: float) -> flo float: The updated position along the hybrid path. """ - signals = HybridPathSignals(path, s) + signals = HybridPathSignals() + signals.update_path(path) + signals.update_s(s) v_s = signals.get_vs(u_desired) s_new = s + (v_s + w) * dt return s_new @@ -314,13 +349,14 @@ class HybridPathSignals: Path (Path): The path object. s (float): The path parameter. """ - 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) + def __init__(self): + # if not isinstance(path, Path): + # raise TypeError("path must be an instance of Path") + self.path = None + self.s = None - def _clamp_s(self, s: float, num_subpaths: int) -> float: + @staticmethod + def _clamp_s(s: float, num_subpaths: int) -> float: """ Ensures s is within the valid range of [0, num_subpaths]. @@ -506,4 +542,29 @@ def get_w(self, mu: float, eta: np.ndarray) -> float: w = (mu / np.linalg.norm(eta_d_s)) * np.dot(eta_d_s, error) return w + + def update_path(self, path: Path) -> None: + """ + Update the path object. + + Args: + path (Path): The new path object. + + Returns: + None + """ + self.path = path + self.update_s(0.) + + def update_s(self, s: float) -> None: + """ + Update the path parameter. + + Args: + s (float): The new path parameter. + + Returns: + None + """ + self.s = self._clamp_s(s, self.path.NumSubpaths) \ No newline at end of file diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py index 971791e3..f97da507 100755 --- a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py +++ b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py @@ -2,14 +2,15 @@ import numpy as np import rclpy from rclpy.node import Node -from geometry_msgs.msg import Pose2D -from std_msgs.msg import Float32 +from geometry_msgs.msg import Pose2D, Point +from std_msgs.msg import Float32, Float64MultiArray from vortex_msgs.msg import HybridpathReference -from vortex_msgs.srv import Waypoint +from vortex_msgs.srv import Waypoint, DesiredVelocity from nav_msgs.msg import Odometry from transforms3d.euler import quat2euler from hybridpath_guidance.hybridpath import HybridPathGenerator, HybridPathSignals from rclpy.qos import QoSProfile, qos_profile_sensor_data, QoSReliabilityPolicy +import threading qos_profile = QoSProfile(depth=1, history=qos_profile_sensor_data.history, @@ -28,8 +29,12 @@ def __init__(self): ]) self.waypoint_server = self.create_service(Waypoint, 'waypoint_list', self.waypoint_callback) + self.u_desired_server = self.create_service(DesiredVelocity, 'u_desired', self.u_desired_callback) self.eta_subscriber_ = self.create_subscription(Odometry, '/seapath/odom/ned', self.eta_callback, qos_profile=qos_profile) self.yaw_publisher = self.create_publisher(Float32, 'yaw', 1) + self.s_publisher = self.create_publisher(Float32, 's', 1) + self.w_publisher = self.create_publisher(Float32, 'w', 1) + self.wp_arr_publisher = self.create_publisher(Float64MultiArray, 'waypoints', 1) self.guidance_publisher = self.create_publisher(HybridpathReference, 'guidance/hybridpath/reference', 1) # Get parameters @@ -39,74 +44,128 @@ def __init__(self): self.mu = self.get_parameter('hybridpath_guidance.mu').get_parameter_value().double_value self.eta = np.zeros(3) - self.u_desired = 0.2 # Desired velocity + self.u_desired = 0.3 # Desired velocity + + self.waypoints = [] + self.path = None + self.s = 0. + self.w = 0. + + # Initialize path generator + self.generator = HybridPathGenerator(self.waypoints, self.path_generator_order, self.lambda_val) + + # Initialize signals + self.signals = HybridPathSignals() # Flags for logging self.waypoints_received = False self.waiting_message_printed = False + self.first_pos_flag = False + self.eta_received = False # Timer for guidance timer_period = 0.1 self.timer = self.create_timer(timer_period, self.guidance_callback) + self.lock = threading.Lock() + + def u_desired_callback(self, request, response): + self.u_desired = request.u_desired + self.get_logger().info(f"Received desired velocity: {self.u_desired}") + response.success = True + return response + + def waypoint_callback(self, request, response): - self.get_logger().info('Received waypoints, generating path...') - self.waypoints = request.waypoint - generator = HybridPathGenerator(self.waypoints, self.path_generator_order, self.lambda_val) - self.path = generator.path + with self.lock: + if self.eta_received: + self.waypoints = [Point(x=self.eta[0], y=self.eta[1])] - self.waypoints_received = True - self.waiting_message_printed = False # Reset this flag to handle multiple waypoint sets + self.get_logger().info('Received waypoints, generating path...') - self.s = 0 - signals = HybridPathSignals(self.path, self.s) - self.w = signals.get_w(self.mu, self.eta) - + new_waypoints = request.waypoint + + for point in new_waypoints: + self.waypoints.append(point) + + self.generator.create_path(self.waypoints) + self.path = self.generator.get_path() + + self.waypoints_received = True + self.waiting_message_printed = False # Reset this flag to handle multiple waypoint sets + self.first_pos_flag = False + + self.s = 0. + self.signals.update_path(self.path) + self.w = self.signals.get_w(self.mu, self.eta) + + wp_arr = Float64MultiArray() + wp_list = self.generator.WP.tolist() + wp_arr.data = [coordinate for wp in wp_list for coordinate in wp] + self.wp_arr_publisher.publish(wp_arr) + response.success = True + return response def eta_callback(self, msg: Odometry): yaw_msg = Float32() self.eta = self.odom_to_eta(msg) - yaw = float(self.eta[2]) - yaw_msg.data = yaw + self.yaw = float(self.eta[2]) + yaw_msg.data = self.yaw self.yaw_publisher.publish(yaw_msg) + self.eta_received = True def guidance_callback(self): - if self.waypoints_received: - self.s = HybridPathGenerator.update_s(self.path, self.dt, self.u_desired, self.s, self.w) - signals = HybridPathSignals(self.path, self.s) - self.w = signals.get_w(self.mu, self.eta) + with self.lock: + if self.waypoints_received: + self.s = self.generator.update_s(self.path, self.dt, self.u_desired, self.s, self.w) + self.signals.update_s(self.s) + self.w = self.signals.get_w(self.mu, self.eta) + + pos = self.signals.get_position() - pos = signals.get_position() - pos_der = signals.get_derivatives()[0] - pos_dder = signals.get_derivatives()[1] + if not self.first_pos_flag: + self.get_logger().info(f"First position: {pos}") + self.first_pos_flag = True - psi = 0.#signals.get_heading() - psi_der = 0.#signals.get_heading_derivative() - psi_dder = 0.#signals.get_heading_second_derivative() + # pos[0] = self.eta[0] + pos_der = self.signals.get_derivatives()[0] + pos_dder = self.signals.get_derivatives()[1] - hp_msg = HybridpathReference() - hp_msg.eta_d = Pose2D(x=pos[0], y=pos[1], theta=psi) - hp_msg.eta_d_s = Pose2D(x=pos_der[0], y=pos_der[1], theta=psi_der) - hp_msg.eta_d_ss = Pose2D(x=pos_dder[0], y=pos_dder[1], theta=psi_dder) + psi = 0. #signals.get_heading() + psi_der = 0.#signals.get_heading_derivative() + psi_dder = 0.#signals.get_heading_second_derivative() - hp_msg.w = signals.get_w(self.mu, self.eta) - hp_msg.v_s = signals.get_vs(self.u_desired) - hp_msg.v_ss = signals.get_vs_derivative(self.u_desired) + hp_msg = HybridpathReference() + hp_msg.eta_d = Pose2D(x=pos[0], y=pos[1], theta=psi) + hp_msg.eta_d_s = Pose2D(x=pos_der[0], y=pos_der[1], theta=psi_der) + hp_msg.eta_d_ss = Pose2D(x=pos_dder[0], y=pos_dder[1], theta=psi_dder) - self.guidance_publisher.publish(hp_msg) + hp_msg.w = self.signals.get_w(self.mu, self.eta) + hp_msg.v_s = self.signals.get_vs(self.u_desired) + hp_msg.v_ss = self.signals.get_vs_derivative(self.u_desired) - if self.s >= self.path.NumSubpaths: - self.waypoints_received = False - self.waiting_message_printed = False - self.get_logger().info('Last waypoint reached') + self.guidance_publisher.publish(hp_msg) - else: - if not self.waiting_message_printed: - self.get_logger().info('Waiting for waypoints to be received') - self.waiting_message_printed = True + if self.s >= self.path.NumSubpaths: + self.waypoints_received = False + self.waiting_message_printed = False + self.get_logger().info('Last waypoint reached') + + else: + if not self.waiting_message_printed: + self.get_logger().info('Waiting for waypoints to be received') + self.waiting_message_printed = True + + s_msg = Float32() + s_msg.data = self.s + self.s_publisher.publish(s_msg) + + w_msg = Float32() + w_msg.data = self.w + self.w_publisher.publish(w_msg) @staticmethod def odom_to_eta(msg: Odometry) -> np.ndarray: @@ -129,7 +188,10 @@ def odom_to_eta(msg: Odometry) -> np.ndarray: # Convert quaternion to Euler angles yaw = quat2euler(orientation_list)[2] + # yaw = np.deg2rad(yaw) + state = np.array([x, y, yaw]) + return state def main(args=None): diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/noisy_odom_publisher.py b/guidance/hybridpath_guidance/hybridpath_guidance/noisy_odom_publisher.py new file mode 100755 index 00000000..5d4395d5 --- /dev/null +++ b/guidance/hybridpath_guidance/hybridpath_guidance/noisy_odom_publisher.py @@ -0,0 +1,57 @@ +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node +from vortex_msgs.srv import Waypoint +from geometry_msgs.msg import Point +from nav_msgs.msg import Odometry +from transforms3d.euler import quat2euler +import numpy as np +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 OdomPublisher(Node): + def __init__(self): + super().__init__('odom_publisher') + + self.eta_odom_publisher = self.create_publisher(Odometry, '/seapath/odom/ned', qos_profile) + + timer_period = 0.01 + self.timer = self.create_timer(timer_period, self.odom_callback) + + self.get_logger().info("Odom publisher started") + + def odom_callback(self): + + noise = np.random.normal(0, 0.005, 3) + + msg = Odometry() + msg.pose.pose.position.x = 0.0 + noise[0] + msg.pose.pose.position.y = 0.0 + noise[1] + msg.pose.pose.position.z = 0.0 + msg.pose.pose.orientation.x = 0.0 + msg.pose.pose.orientation.y = 0.0 + msg.pose.pose.orientation.z = 0.0 + msg.pose.pose.orientation.w = 1.0 + msg.twist.twist.linear.x = 0.0 + noise[0] + msg.twist.twist.linear.y = 0.0 + noise[1] + msg.twist.twist.linear.z = 0.0 + msg.twist.twist.angular.x = 0.0 + msg.twist.twist.angular.y = 0.0 + msg.twist.twist.angular.z = 0.0 + noise[2] + + self.eta_odom_publisher.publish(msg) + + +def main(args=None): + rclpy.init(args=args) + node = OdomPublisher() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/u_desired_node.py b/guidance/hybridpath_guidance/hybridpath_guidance/u_desired_node.py new file mode 100644 index 00000000..033afd22 --- /dev/null +++ b/guidance/hybridpath_guidance/hybridpath_guidance/u_desired_node.py @@ -0,0 +1,48 @@ +import rclpy +from rclpy.node import Node +from vortex_msgs.srv import DesiredVelocity + +class DesiredVelocityClient(Node): + def __init__(self): + super().__init__('desired_velocity_client') + self.client = self.create_client(DesiredVelocity, 'u_desired') + + while not self.client.wait_for_service(timeout_sec=2.0): + self.get_logger().info('service not available, waiting again...') + + self.send_request() + + def send_request(self): + req = DesiredVelocity.Request() + req.u = 0.3 + self.get_logger().info(f'Sending request: {req}') + self.future = self.client.call_async(req) + self.future.add_done_callback(self.get_response) + + def get_response(self, future): + try: + response = future.result() + self.get_logger().info(f'Received response: {response}') + if response.success: + self.destroy_node() + rclpy.shutdown() + + except Exception as e: + self.get_logger().error('Service call failed %r' % (e,)) + +def main(args=None): + rclpy.init(args=args) + + client = DesiredVelocityClient() + + try: + rclpy.spin(client) + except Exception as e: + client.get_logger().error('Error in DesiredVelocityClient: %r' % (e,)) + finally: + if rclpy.ok(): + client.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/waypoint_node.py b/guidance/hybridpath_guidance/hybridpath_guidance/waypoint_node.py index d9e024bd..59b5df38 100755 --- a/guidance/hybridpath_guidance/hybridpath_guidance/waypoint_node.py +++ b/guidance/hybridpath_guidance/hybridpath_guidance/waypoint_node.py @@ -39,7 +39,7 @@ def eta_callback(self, msg: Odometry): def send_request(self): if self.eta_received: req = Waypoint.Request() - wp_list = [[self.eta[0], self.eta[1]], [self.eta[0] + 3., self.eta[1]]] + wp_list = [[self.eta[0], self.eta[1]], [self.eta[0] + 3., self.eta[1] + 0.]] req.waypoint = [Point(x=float(wp[0]), y=float(wp[1]), z=0.0) for wp in wp_list] self.get_logger().info(f'Sending request: {req}') self.future = self.client.call_async(req) diff --git a/motion/hybridpath_controller/config/hybridpath_controller_config.yaml b/motion/hybridpath_controller/config/hybridpath_controller_config.yaml index ef6650ff..f01ea966 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: [0.5, 0.5, 0.5] # First gain matrix - K2_diag: [20.0, 20.0, 20.0] # Second gain matrix \ No newline at end of file + K1_diag: [1., 1., 1.] # First gain matrix + K2_diag: [40.0, 30.0, 20.0] # Second gain matrix \ No newline at end of file diff --git a/motion/hybridpath_controller/hybridpath_controller/adaptive_backstep.py b/motion/hybridpath_controller/hybridpath_controller/adaptive_backstep.py index ff95a51c..ecc0008e 100644 --- a/motion/hybridpath_controller/hybridpath_controller/adaptive_backstep.py +++ b/motion/hybridpath_controller/hybridpath_controller/adaptive_backstep.py @@ -4,7 +4,13 @@ from transforms3d.euler import quat2euler class AdaptiveBackstep: - def __init__(self, K1: np.ndarray, K2: np.ndarray, M: np.ndarray, D: np.ndarray) -> None: + def __init__(self): + self.K_1 = np.eye(3) + self.K_2 = np.eye(3) + self.M = np.eye(3) + self.D = np.eye(3) + + def update_parameters(self, K1: np.ndarray, K2: np.ndarray, M: np.ndarray, D: np.ndarray) -> None: self.K_1 = K1 self.K_2 = K2 self.M = M @@ -27,6 +33,7 @@ def control_law(self, state: Odometry, reference: HybridpathReference) -> np.nda # Extract values from the state and reference eta = state[:3] + # eta[0] = 0. nu = state[3:] w = reference.w v_s = reference.v_s @@ -54,8 +61,34 @@ def control_law(self, state: Odometry, reference: HybridpathReference) -> np.nda tau = -self.K_2 @ z2 + self.calculate_coriolis_matrix(nu) + self.D @ nu + self.M @ sigma1 + self.M @ ds_alpha1 * (v_s + w) + self.eta_error = eta_error + self.z1 = z1 + self.alpha1 = alpha1 + self.z2 = z2 + self.ds_alpha1 = ds_alpha1 + self.sigma1 = sigma1 + return tau + def get_eta_error(self): + return self.eta_error + + def get_z1(self): + return self.z1 + + def get_alpha1(self): + return self.alpha1 + + def get_z2(self): + return self.z2 + + def get_sigma1(self): + return self.sigma1 + + def get_ds_alpha1(self): + return self.ds_alpha1 + + @staticmethod def calculate_coriolis_matrix(nu: np.ndarray) -> np.ndarray: """ @@ -66,7 +99,7 @@ def calculate_coriolis_matrix(nu: np.ndarray) -> np.ndarray: [0, 0, 5.5], [82.5, -5.5, 0] ]) - return C_A @ nu + return (C_A @ nu) * 0 @staticmethod def rotationmatrix_in_yaw_transpose(psi: float) -> np.ndarray: @@ -118,6 +151,8 @@ def odom_to_state(msg: Odometry) -> np.ndarray: # Convert quaternion to Euler angles yaw = quat2euler(orientation_list)[2] + # yaw = np.deg2rad(yaw) + u = msg.twist.twist.linear.x v = msg.twist.twist.linear.y r = msg.twist.twist.angular.z diff --git a/motion/hybridpath_controller/hybridpath_controller/hybridpath_controller_node.py b/motion/hybridpath_controller/hybridpath_controller/hybridpath_controller_node.py index 07c69eaa..86718344 100755 --- a/motion/hybridpath_controller/hybridpath_controller/hybridpath_controller_node.py +++ b/motion/hybridpath_controller/hybridpath_controller/hybridpath_controller_node.py @@ -7,7 +7,9 @@ from geometry_msgs.msg import Wrench from nav_msgs.msg import Odometry from vortex_msgs.msg import HybridpathReference +from std_msgs.msg import Float64MultiArray from rclpy.qos import QoSProfile, qos_profile_sensor_data, QoSReliabilityPolicy +from rcl_interfaces.msg import SetParametersResult qos_profile = QoSProfile(depth=1, history=qos_profile_sensor_data.history, @@ -25,31 +27,61 @@ def __init__(self): ('physical.damping_matrix_diag', [10.0, 10.0, 5.0]) ]) + self.parameters_updated = False + self.state_subscriber_ = self.state_subscriber_ = self.create_subscription(Odometry, '/seapath/odom/ned', self.state_callback, qos_profile=qos_profile) self.hpref_subscriber_ = self.create_subscription(HybridpathReference, 'guidance/hybridpath/reference', self.reference_callback, 1) self.wrench_publisher_ = self.create_publisher(Wrench, 'thrust/wrench_input', 1) - # Get parameters + # Debug publishers + self.eta_error_publisher = self.create_publisher(Float64MultiArray, 'eta_error', 10) + self.z1_publisher = self.create_publisher(Float64MultiArray, 'z1', 10) + self.alpha1_publisher = self.create_publisher(Float64MultiArray, 'alpha1', 10) + self.z2_publisher = self.create_publisher(Float64MultiArray, 'z2', 10) + self.ds_alpha1_publisher = self.create_publisher(Float64MultiArray, 'ds_alpha1', 10) + self.sigma1_publisher = self.create_publisher(Float64MultiArray, 'sigma1', 10) + self.tau_publisher = self.create_publisher(Float64MultiArray, 'tau', 10) + + self.AB_controller_ = AdaptiveBackstep() + + self.update_controller_parameters() + + controller_period = 0.1 + self.controller_timer_ = self.create_timer(controller_period, self.controller_callback) + self.add_on_set_parameters_callback(self.parameter_callback) + + self.get_logger().info("hybridpath_controller_node started") + + def update_controller_parameters(self): + K1_diag = self.get_parameter('hybridpath_controller.K1_diag').get_parameter_value().double_array_value K2_diag = self.get_parameter('hybridpath_controller.K2_diag').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 - # Transform parameters to diagonal matrices K1 = np.diag(K1_diag) K2 = np.diag(K2_diag) D = np.diag(D_diag) - - # Reshape M to a 3x3 matrix M = np.reshape(M, (3, 3)) - # Create controller object - self.AB_controller_ = AdaptiveBackstep(K1, K2, M, D) + self.AB_controller_.update_parameters(K1, K2, M, D) - controller_period = 0.1 - self.controller_timer_ = self.create_timer(controller_period, self.controller_callback) + self.get_logger().info("Updated controller parameters") - self.get_logger().info("hybridpath_controller_node started") + def parameter_callback(self, params): + self.parameters_updated = True + for param in params: + if param.name == 'hybridpath_controller.K1_diag': + self.get_logger().info(f"Updated K1_diag to {param.value}") + elif param.name == 'hybridpath_controller.K2_diag': + self.get_logger().info(f"Updated K2_diag to {param.value}") + elif param.name == 'physical.damping_matrix_diag': + self.get_logger().info(f"Updated damping_matrix_diag to {param.value}") + elif param.name == 'physical.inertia_matrix': + self.get_logger().info(f"Updated inertia_matrix to {param.value}") + + # self.update_controller_parameters() + return SetParametersResult(successful=True) def state_callback(self, msg: Odometry): """ @@ -64,6 +96,10 @@ def controller_callback(self): """ Callback function for the controller timer. This function calculates the control input and publishes the control input. """ + if self.parameters_updated: + self.update_controller_parameters() + self.parameters_updated = False + if hasattr(self, 'state_odom') and hasattr(self, 'reference'): control_input = self.AB_controller_.control_law(self.state_odom, self.reference) wrench_msg = Wrench() @@ -72,6 +108,36 @@ def controller_callback(self): wrench_msg.torque.z = control_input[2] self.wrench_publisher_.publish(wrench_msg) + # Debug publishers + eta_error_msg = Float64MultiArray() + eta_error_msg.data = self.AB_controller_.get_eta_error().tolist() + self.eta_error_publisher.publish(eta_error_msg) + + z1_msg = Float64MultiArray() + z1_msg.data = self.AB_controller_.get_z1().tolist() + self.z1_publisher.publish(z1_msg) + + alpha1_msg = Float64MultiArray() + alpha1_msg.data = self.AB_controller_.get_alpha1().tolist() + self.alpha1_publisher.publish(alpha1_msg) + + z2_msg = Float64MultiArray() + z2_msg.data = self.AB_controller_.get_z2().tolist() + self.z2_publisher.publish(z2_msg) + + ds_alpha1_msg = Float64MultiArray() + ds_alpha1_msg.data = self.AB_controller_.get_ds_alpha1().tolist() + self.ds_alpha1_publisher.publish(ds_alpha1_msg) + + sigma1_msg = Float64MultiArray() + sigma1_msg.data = self.AB_controller_.get_sigma1().tolist() + self.sigma1_publisher.publish(sigma1_msg) + + tau_msg = Float64MultiArray() + tau_msg.data = control_input.tolist() + self.tau_publisher.publish(tau_msg) + + def main(args=None): rclpy.init(args=args) node = HybridPathControllerNode()