diff --git a/guidance/dp_guidance/CMakeLists.txt b/guidance/dp_guidance/CMakeLists.txt index 15c75d79..7cc58d78 100644 --- a/guidance/dp_guidance/CMakeLists.txt +++ b/guidance/dp_guidance/CMakeLists.txt @@ -24,12 +24,4 @@ install(PROGRAMS DESTINATION lib/${PROJECT_NAME} ) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - find_package(ament_cmake_pytest REQUIRED) - ament_add_pytest_test(python_tests tests) - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) -endif() - ament_package() \ 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 index 380e0ad7..a416f952 100755 --- a/guidance/dp_guidance/dp_guidance/dp_guidance_node.py +++ b/guidance/dp_guidance/dp_guidance/dp_guidance_node.py @@ -1,15 +1,15 @@ #!/usr/bin/env python3 + import numpy as np import rclpy from rclpy.node import Node from geometry_msgs.msg import Pose2D -from vortex_msgs.msg import HybridpathReference 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 -from dp_guidance.hybridpath import HybridPathGenerator, HybridPathSignals +from dp_guidance.conversions import odometrymsg_to_state, state_to_odometrymsg +from dp_guidance.reference_filter import ReferenceFilter class Guidance(Node): def __init__(self): @@ -31,16 +31,22 @@ def __init__(self): self.waypoints_received = False self.waiting_message_printed = False - self.eta_d = np.array([0, 0, 0]) + 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.get_logger().info('Received waypoints, generating path...') self.waypoints = request.waypoint - self.eta_d = self.waypoints[0] # Choosing first waypoint as desired eta + 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 @@ -51,9 +57,19 @@ def eta_callback(self, msg: Odometry): def guidance_callback(self): if self.waypoints_received: - - odom_msg = Odometry() + 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: diff --git a/motion/pid_controller/pid_controller/pid_controller_node.py b/motion/pid_controller/pid_controller/pid_controller_node.py index 3f193d60..6c7d0123 100644 --- a/motion/pid_controller/pid_controller/pid_controller_node.py +++ b/motion/pid_controller/pid_controller/pid_controller_node.py @@ -39,8 +39,6 @@ def __init__(self): self.x_ref = np.array([0, 0, 0]) self.state = np.array([0, 0, 0, 0, 0, 0]) - self.x_ref = np.array([10, 10, 0]) - self.enabled = False self.controller_period = 0.1 diff --git a/simulation/asv_simulator/asv_simulator/asv_simulator_node.py b/simulation/asv_simulator/asv_simulator/asv_simulator_node.py index e9c4a5b4..5e51afe4 100644 --- a/simulation/asv_simulator/asv_simulator/asv_simulator_node.py +++ b/simulation/asv_simulator/asv_simulator/asv_simulator_node.py @@ -51,7 +51,8 @@ def __init__(self): # subscribe to thrust input and guidance self.wrench_subscriber_ = self.create_subscription(Wrench, "thrust/wrench_input", self.wrench_input_cb, 1) - self.guidance_subscriber_ = self.create_subscription(HybridpathReference, 'guidance/hybridpath/reference', self.guidance_cb, 1) + self.guidance_hp_subscriber_ = self.create_subscription(HybridpathReference, 'guidance/hybridpath/reference', self.guidance_hp_cb, 1) + self.guidance_dp_subscriber_ = self.create_subscription(Odometry, 'guidance/dp/reference', self.guidance_dp_cb, 1) # publish state (seapath) self.state_publisher_ = self.create_publisher(Odometry, "/sensor/seapath/odom/ned", 1) @@ -129,12 +130,19 @@ def show_progressbar(self): bar = '[' + '=' * filled_length + ' ' * (10 - filled_length) + ']' self.get_logger().info(f"Progress: {bar} {progress:.1f}%") - def guidance_cb(self, msg): + def guidance_hp_cb(self, msg): if isinstance(msg, HybridpathReference): self.x_ref[:3] = np.array([msg.eta_d.x, msg.eta_d.y, msg.eta_d.theta]) self.yaw_ref_publisher_.publish(Float32(data=self.x_ref[2])) else: self.get_logger().error(f"Received message of type {type(msg).__name__}, expected HybridpathReference") + + def guidance_dp_cb(self, msg): + if isinstance(msg, Odometry): + self.x_ref[:3] = odometrymsg_to_state(msg)[:3] + self.yaw_ref_publisher_.publish(Float32(data=self.x_ref[2])) + else: + self.get_logger().error(f"Received message of type {type(msg).__name__}, expected Odometry") def wrench_input_cb(self, msg): self.tau = np.array([msg.force.x, msg.force.y, msg.torque.z]) @@ -194,8 +202,8 @@ def plot_matplotlib(self): tau_time = np.linspace(0, self.T, len(self.tau_history)) plt.figure() - plt.plot(state_x, state_y, label='State Path') - plt.plot(ref_x, ref_y, label='Reference Path') + plt.plot(state_x, state_y, label='State Path') + plt.plot(ref_x, ref_y, label='Reference Path', linestyle='--') plt.title('ASV Path') plt.xlabel('X') plt.ylabel('Y') diff --git a/simulation/asv_simulator/config/asv_sim_config.yaml b/simulation/asv_simulator/config/asv_sim_config.yaml index 6820def0..3ee9bb62 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: false - T: 200 + T: 700 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