From 539d0252f2e90f88f5b92343e0025fca95b439c2 Mon Sep 17 00:00:00 2001 From: Andeshog <157955984+Andeshog@users.noreply.github.com> Date: Mon, 29 Apr 2024 12:36:01 +0200 Subject: [PATCH] Added QoS profile to seapath/odom subscription --- .../hybridpath_guidance/hybridpath_guidance_node.py | 8 ++++++-- .../hybridpath_guidance/waypoint_node.py | 9 +++++++-- .../hybridpath_controller/hybridpath_controller_node.py | 7 ++++++- 3 files changed, 19 insertions(+), 5 deletions(-) diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py index 2022db46..5fe957df 100755 --- 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 index ff8dc843..d9e024bd 100755 --- 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 index e9a88bde..07c69eaa 100755 --- 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)