diff --git a/guidance/hybridpath_guidance/CMakeLists.txt b/guidance/hybridpath_guidance/CMakeLists.txt index 0fca6d21..e16c1b10 100644 --- a/guidance/hybridpath_guidance/CMakeLists.txt +++ b/guidance/hybridpath_guidance/CMakeLists.txt @@ -21,6 +21,7 @@ install(DIRECTORY install(PROGRAMS hybridpath_guidance/hybridpath_guidance_node.py + hybridpath_guidance/waypoint_node.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 72488399..20fa168f 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.03 # Tuning parameter \ No newline at end of file + 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 diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py index 36255f0d..2022db46 100755 --- a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py +++ b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py @@ -19,11 +19,11 @@ def __init__(self): ('hybridpath_guidance.lambda_val', 0.15), ('hybridpath_guidance.path_generator_order', 1), ('hybridpath_guidance.dt', 0.1), - ('hybridpath_guidance.mu', 0.03) + ('hybridpath_guidance.mu', 0.2) ]) self.waypoint_server = self.create_service(Waypoint, 'waypoint_list', self.waypoint_callback) - self.eta_subscriber_ = self.create_subscription(Odometry, '/sensor/seapath/odom/ned', self.eta_callback, 1) + self.eta_subscriber_ = self.create_subscription(Odometry, '/seapath/odom/ned', self.eta_callback, 1) self.guidance_publisher = self.create_publisher(HybridpathReference, 'guidance/hybridpath/reference', 1) # Get parameters @@ -33,7 +33,7 @@ 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.25 # Desired velocity + self.u_desired = 0.2 # Desired velocity # Flags for logging self.waypoints_received = False diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/waypoint_node.py b/guidance/hybridpath_guidance/hybridpath_guidance/waypoint_node.py new file mode 100755 index 00000000..bae7f73b --- /dev/null +++ b/guidance/hybridpath_guidance/hybridpath_guidance/waypoint_node.py @@ -0,0 +1,91 @@ +#!/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 + +class WaypointClient(Node): + def __init__(self): + super().__init__('waypoint_client') + self.eta_odom = self.create_subscription(Odometry, '/seapath/odom/ned', self.eta_callback, 1) + + self.eta_received = False + + while not self.eta_received: + self.get_logger().info('Waiting for eta...') + rclpy.spin_once(self) + + self.client = self.create_client(Waypoint, 'waypoint_list') + + while not self.client.wait_for_service(timeout_sec=2.0): + self.get_logger().info('service not available, waiting again...') + + self.send_request() + + def eta_callback(self, msg: Odometry): + self.eta = self.odom_to_eta(msg) + #self.get_logger().info(f'Received eta {self.eta}') + self.eta_received = True + + 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]]] + 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) + 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,)) + + @staticmethod + def odom_to_eta(msg: Odometry) -> np.ndarray: + """ + Converts an Odometry message to 3DOF eta vector. + + Args: + msg (Odometry): The Odometry message to convert. + + Returns: + np.ndarray: The eta vector. + """ + 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 + yaw = quat2euler(orientation_list)[2] + + state = np.array([x, y, yaw]) + return state + +def main(args=None): + rclpy.init(args=args) + waypoint_client_node = WaypointClient() + try: + rclpy.spin(waypoint_client_node) + except Exception as e: + waypoint_client_node.get_logger().error(f'Unhandled exception: {e}') + finally: + if rclpy.ok(): + waypoint_client_node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/motion/hybridpath_controller/config/hybridpath_controller_config.yaml b/motion/hybridpath_controller/config/hybridpath_controller_config.yaml index e7ac823a..14a63975 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: [40.0, 40.0, 40.0] # Second gain matrix \ No newline at end of file diff --git a/motion/hybridpath_controller/hybridpath_controller/hybridpath_controller_node.py b/motion/hybridpath_controller/hybridpath_controller/hybridpath_controller_node.py index d74f1abb..e9a88bde 100755 --- a/motion/hybridpath_controller/hybridpath_controller/hybridpath_controller_node.py +++ b/motion/hybridpath_controller/hybridpath_controller/hybridpath_controller_node.py @@ -20,7 +20,7 @@ def __init__(self): ('physical.damping_matrix_diag', [10.0, 10.0, 5.0]) ]) - self.state_subscriber_ = self.state_subscriber_ = self.create_subscription(Odometry, '/sensor/seapath/odom/ned', self.state_callback, 1) + self.state_subscriber_ = self.state_subscriber_ = self.create_subscription(Odometry, '/seapath/odom/ned', self.state_callback, 1) 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)