From 17eefa747236147e14f168b970cb684aeb5ca5ae Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Mon, 13 May 2024 18:48:07 +0200 Subject: [PATCH 1/4] Fix player QoS MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- rqt_bag/src/rqt_bag/player.py | 26 ++++++++++++++++++++++++-- 1 file changed, 24 insertions(+), 2 deletions(-) diff --git a/rqt_bag/src/rqt_bag/player.py b/rqt_bag/src/rqt_bag/player.py index f7321eb..c368115 100644 --- a/rqt_bag/src/rqt_bag/player.py +++ b/rqt_bag/src/rqt_bag/player.py @@ -129,8 +129,30 @@ def message_viewed(self, bag, entry): # Create publisher if this is the first message on the topic if topic not in self._publishers: - topic_metadata = bag.get_topic_metadata(topic) - self.create_publisher(topic, ros_message, topic_metadata.offered_qos_profiles) + rosbag2_qos = bag.get_topic_metadata(entry.topic).offered_qos_profiles[0] + + reliability = ReliabilityPolicy.SYSTEM_DEFAULT + if rosbag2_qos.reliability() == rmw_qos_reliability_policy_t.RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT: + reliability = ReliabilityPolicy.BEST_EFFORT + elif rosbag2_qos.reliability() == rmw_qos_reliability_policy_t.RMW_QOS_POLICY_RELIABILITY_RELIABLE: + reliability = ReliabilityPolicy.RELIABLE + + durability = DurabilityPolicy.SYSTEM_DEFAULT + if rosbag2_qos.durability() == rmw_qos_durability_policy_t.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL: + durability = DurabilityPolicy.TRANSIENT_LOCAL + elif rosbag2_qos.reliability() == rmw_qos_durability_policy_t.RMW_QOS_POLICY_DURABILITY_VOLATILE: + durability = DurabilityPolicy.VOLATILE + + history = HistoryPolicy.SYSTEM_DEFAULT + if rosbag2_qos.history() == rmw_qos_history_policy_t.RMW_QOS_POLICY_HISTORY_KEEP_LAST: + history = DurabilityPolicy.KEEP_LAST + elif rosbag2_qos.history() == rmw_qos_history_policy_t.RMW_QOS_POLICY_HISTORY_KEEP_ALL: + history = DurabilityPolicy.KEEP_ALL + + qos_profile_publisher = QoSProfile( + depth=rosbag2_qos.depth(), reliability=reliability, durability=durability, history=history) + + self.create_publisher(entry.topic, ros_message, qos_profile_publisher) if self._publish_clock: time_msg = Time() From c0c1d0c2553fc6e6a70a3d923563ff0ebd5deb34 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Mon, 13 May 2024 18:49:36 +0200 Subject: [PATCH 2/4] included missing imports MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- rqt_bag/src/rqt_bag/player.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/rqt_bag/src/rqt_bag/player.py b/rqt_bag/src/rqt_bag/player.py index c368115..f5e4062 100644 --- a/rqt_bag/src/rqt_bag/player.py +++ b/rqt_bag/src/rqt_bag/player.py @@ -35,8 +35,13 @@ from builtin_interfaces.msg import Time from python_qt_binding.QtCore import QObject from rclpy import logging -from rclpy.qos import QoSProfile +from rclpy.qos import QoSProfile, DurabilityPolicy, HistoryPolicy, ReliabilityPolicy + + from .qos import yaml_to_qos_profiles, gen_publisher_qos_profile +from .qos import get_qos_profiles_for_topic +from rosbag2_py import rmw_qos_durability_policy_t, rmw_qos_history_policy_t, rmw_qos_reliability_policy_t + CLOCK_TOPIC = "/clock" From b303343cc567899863616349dcb7c309abe63e30 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Thu, 16 May 2024 16:08:51 +0200 Subject: [PATCH 3/4] use new function convert_rosbag_qos_to_rlcpy_qos MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- rqt_bag/src/rqt_bag/player.py | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/rqt_bag/src/rqt_bag/player.py b/rqt_bag/src/rqt_bag/player.py index 0b446f6..d534ff7 100644 --- a/rqt_bag/src/rqt_bag/player.py +++ b/rqt_bag/src/rqt_bag/player.py @@ -34,6 +34,7 @@ from builtin_interfaces.msg import Time from python_qt_binding.QtCore import QObject from rclpy.qos import QoSProfile +from rosbag2_py import convert_rosbag_qos_to_rlcpy_qos CLOCK_TOPIC = "/clock" @@ -125,14 +126,9 @@ def message_viewed(self, bag, entry): # Create publisher if this is the first message on the topic if entry.topic not in self._publishers: - # TODO(clalancette): We should try to get the topic metadata via a call to - # bag.get_topic_metadata() so we can recreate the publisher with the same QoS profile. - # Unfortunately, as it stands that returns a rosbag2_py._storage.QoS object, and the - # eventual call to 'create_publisher' needs an rclpy.QoSProfile object. - # We can't convert between them because the rosbag2_py._storage.QoS object has no - # introspection capabilities, nor a method to convert itself to a QoSProfile object. - # So for now, we just use a default QoSProfile. - self.create_publisher(entry.topic, ros_message, QoSProfile(depth=10)) + rosbag2_qos = bag.get_topic_metadata(entry.topic).offered_qos_profiles[0] + qos = convert_rosbag_qos_to_rlcpy_qos(rosbag2_qos) + self.create_publisher(entry.topic, ros_message, qos) if self._publish_clock: time_msg = Time() From 45e40debfdaf6fb6ad205d4ddbd8c0901e830756 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 22 May 2024 15:21:14 +0200 Subject: [PATCH 4/4] updated function name MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- rqt_bag/src/rqt_bag/player.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rqt_bag/src/rqt_bag/player.py b/rqt_bag/src/rqt_bag/player.py index d534ff7..0ee3855 100644 --- a/rqt_bag/src/rqt_bag/player.py +++ b/rqt_bag/src/rqt_bag/player.py @@ -34,7 +34,7 @@ from builtin_interfaces.msg import Time from python_qt_binding.QtCore import QObject from rclpy.qos import QoSProfile -from rosbag2_py import convert_rosbag_qos_to_rlcpy_qos +from rosbag2_py import convert_rclcpp_qos_to_rclpy_qos CLOCK_TOPIC = "/clock" @@ -127,7 +127,7 @@ def message_viewed(self, bag, entry): # Create publisher if this is the first message on the topic if entry.topic not in self._publishers: rosbag2_qos = bag.get_topic_metadata(entry.topic).offered_qos_profiles[0] - qos = convert_rosbag_qos_to_rlcpy_qos(rosbag2_qos) + qos = convert_rclcpp_qos_to_rclpy_qos(rosbag2_qos) self.create_publisher(entry.topic, ros_message, qos) if self._publish_clock: