Added QoS profile to seapath/odom subscription

vortexntnu · Apr 29, 2024 · 539d025 · 539d025
1 parent 4a1110f
commit 539d025
Show file tree

Hide file tree

Showing 3 changed files with 19 additions and 5 deletions.
diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py
@@ -7,8 +7,12 @@
 from vortex_msgs.srv import Waypoint
 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
+
+
+qos_profile = QoSProfile(depth=1, history=qos_profile_sensor_data.history, 
+                         reliability=QoSReliabilityPolicy.BEST_EFFORT)
 
 class Guidance(Node):
     def __init__(self):
@@ -23,7 +27,7 @@ def __init__(self):
             ])
 
         self.waypoint_server = self.create_service(Waypoint, 'waypoint_list', self.waypoint_callback)
-        self.eta_subscriber_ = self.create_subscription(Odometry, '/seapath/odom/ned', self.eta_callback, 1)
+        self.eta_subscriber_ = self.create_subscription(Odometry, '/seapath/odom/ned', self.eta_callback, qos_profile=qos_profile)
         self.guidance_publisher = self.create_publisher(HybridpathReference, 'guidance/hybridpath/reference', 1)
 
         # Get parameters

diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/waypoint_node.py b/guidance/hybridpath_guidance/hybridpath_guidance/waypoint_node.py
@@ -7,11 +7,16 @@
 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 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_odom = self.create_subscription(Odometry, '/seapath/odom/ned', self.eta_callback, qos_profile=qos_profile)
 
         self.eta_received = False
 
@@ -34,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] + 3]]
+            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)

diff --git a/motion/hybridpath_controller/hybridpath_controller/hybridpath_controller_node.py b/motion/hybridpath_controller/hybridpath_controller/hybridpath_controller_node.py
@@ -7,6 +7,11 @@
 from geometry_msgs.msg import Wrench
 from nav_msgs.msg import Odometry
 from vortex_msgs.msg import HybridpathReference
+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 HybridPathControllerNode(Node):
     def __init__(self):
@@ -20,7 +25,7 @@ def __init__(self):
                 ('physical.damping_matrix_diag', [10.0, 10.0, 5.0])
             ])
 
-        self.state_subscriber_ = self.state_subscriber_ = self.create_subscription(Odometry, '/seapath/odom/ned', self.state_callback, 1)
+        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)