Skip to content

Commit

Permalink
Fixed topic names and initial tuning
Browse files Browse the repository at this point in the history
  • Loading branch information
Andeshog committed Apr 29, 2024
1 parent 0f9feb3 commit cff5d3c
Show file tree
Hide file tree
Showing 6 changed files with 99 additions and 7 deletions.
1 change: 1 addition & 0 deletions guidance/hybridpath_guidance/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@ install(DIRECTORY

install(PROGRAMS
hybridpath_guidance/hybridpath_guidance_node.py
hybridpath_guidance/waypoint_node.py
DESTINATION lib/${PROJECT_NAME}
)

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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
mu: 0.15 # Tuning parameter, changes how much the path will wait for the vessel if it lags behind
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand Down
91 changes: 91 additions & 0 deletions guidance/hybridpath_guidance/hybridpath_guidance/waypoint_node.py
Original file line number Diff line number Diff line change
@@ -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()
Original file line number Diff line number Diff line change
@@ -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
K1_diag: [1.0, 1.0, 1.0] # First gain matrix
K2_diag: [40.0, 40.0, 40.0] # Second gain matrix
Original file line number Diff line number Diff line change
Expand Up @@ -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)

Expand Down

0 comments on commit cff5d3c

Please sign in to comment.