From ad65f939abd75404dc55d92ae26c8cc516d9f05a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Thu, 20 Jul 2023 17:27:24 +0200 Subject: [PATCH 1/2] Publish TwistStamped message too MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- src/rqt_robot_steering/robot_steering.py | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/src/rqt_robot_steering/robot_steering.py b/src/rqt_robot_steering/robot_steering.py index 4e3458c..13ec067 100644 --- a/src/rqt_robot_steering/robot_steering.py +++ b/src/rqt_robot_steering/robot_steering.py @@ -31,7 +31,7 @@ import os from ament_index_python import get_resource -from geometry_msgs.msg import Twist +from geometry_msgs.msg import Twist, TwistStamped from python_qt_binding import loadUi from python_qt_binding.QtCore import Qt, QTimer, Slot from python_qt_binding.QtGui import QKeySequence @@ -51,6 +51,7 @@ def __init__(self, context): self._node = context.node self._publisher = None + self._publisher_stamped = None self._widget = QWidget() _, package_path = get_resource('packages', 'rqt_robot_steering') @@ -181,6 +182,8 @@ def _on_topic_changed(self, topic): if topic == '': return self._publisher = self._node.create_publisher(Twist, topic, qos_profile=QoSProfile(depth=10)) + self._publisher_stamped = self._node.create_publisher( + TwistStamped, topic + '_stamped', qos_profile=QoSProfile(depth=10)) def _on_stop_pressed(self): # If the current value of sliders is zero directly send stop twist msg @@ -262,8 +265,9 @@ def _on_parameter_changed(self): self._widget.z_angular_slider.value() / RobotSteering.slider_factor) def _send_twist(self, x_linear, z_angular): - if self._publisher is None: + if self._publisher_stamped is None and self._publisher_stamped is None: return + twist = Twist() twist.linear.x = x_linear twist.linear.y = 0.0 @@ -272,19 +276,29 @@ def _send_twist(self, x_linear, z_angular): twist.angular.y = 0.0 twist.angular.z = z_angular + twist_stamped = TwistStamped() + twist_stamped.twist = twist + twist_stamped.header.stamp = self._node.get_clock().now().to_msg() + twist_stamped.header.frame_id = '' + # Only send the zero command once so other devices can take control if x_linear == 0.0 and z_angular == 0.0: if not self.zero_cmd_sent: self.zero_cmd_sent = True self._publisher.publish(twist) + self._publisher_stamped.publish(twist_stamped) else: self.zero_cmd_sent = False self._publisher.publish(twist) + self._publisher_stamped.publish(twist_stamped) def _unregister_publisher(self): if self._publisher is not None: self._node.destroy_publisher(self._publisher) self._publisher = None + if self._publisher_stamped is not None: + self._node.destroy_publisher(self._publisher_stamped) + self._publisher_stamped = None def shutdown_plugin(self): self._update_parameter_timer.stop() From 1be4a116ad75a7cf87a08979407a4e3ceaf1f78f Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sat, 23 Mar 2024 19:29:04 +0000 Subject: [PATCH 2/2] Add a stamped checkbox and publish only one topic --- resource/RobotSteering.ui | 10 ++++ src/rqt_robot_steering/robot_steering.py | 60 +++++++++++++++++++++--- 2 files changed, 63 insertions(+), 7 deletions(-) diff --git a/resource/RobotSteering.ui b/resource/RobotSteering.ui index d84a751..87fbae8 100644 --- a/resource/RobotSteering.ui +++ b/resource/RobotSteering.ui @@ -26,6 +26,16 @@ + + + + stamped + + + true + + + diff --git a/src/rqt_robot_steering/robot_steering.py b/src/rqt_robot_steering/robot_steering.py index 13ec067..2054bac 100644 --- a/src/rqt_robot_steering/robot_steering.py +++ b/src/rqt_robot_steering/robot_steering.py @@ -52,10 +52,12 @@ def __init__(self, context): self._publisher = None self._publisher_stamped = None + self._use_stamped = True self._widget = QWidget() _, package_path = get_resource('packages', 'rqt_robot_steering') - ui_file = os.path.join(package_path, 'share', 'rqt_robot_steering', 'resource', 'RobotSteering.ui') + ui_file = os.path.join( + package_path, 'share', 'rqt_robot_steering', 'resource', 'RobotSteering.ui') loadUi(ui_file, self._widget) self._widget.setObjectName('RobotSteeringUi') if context.serial_number() > 1: @@ -65,6 +67,8 @@ def __init__(self, context): self._widget.topic_line_edit.textChanged.connect( self._on_topic_changed) + self._widget.stamped_check_box.stateChanged.connect( + self._on_stamped_cb_changed) self._widget.stop_push_button.pressed.connect(self._on_stop_pressed) self._widget.x_linear_slider.valueChanged.connect( @@ -181,9 +185,42 @@ def _on_topic_changed(self, topic): self._unregister_publisher() if topic == '': return - self._publisher = self._node.create_publisher(Twist, topic, qos_profile=QoSProfile(depth=10)) - self._publisher_stamped = self._node.create_publisher( - TwistStamped, topic + '_stamped', qos_profile=QoSProfile(depth=10)) + try: + if self._use_stamped: + self._publisher_stamped = self._node.create_publisher( + TwistStamped, topic, qos_profile=QoSProfile(depth=10)) + else: + self._publisher = self._node.create_publisher( + Twist, topic, qos_profile=QoSProfile(depth=10)) + except Exception as e: + print('Error creating publisher: %s' % e) + + @Slot(int) + def _on_stamped_cb_changed(self, state): + state = int(state) + self._unregister_publisher() + self._use_stamped = state + # we can't change this in the same slot or we get a type error from rcl + self._update_topic_type_timer = QTimer(self) + self._update_topic_type_timer.timeout.connect( + self._on_topic_type_changed) + self._update_topic_type_timer.start(100) + + def _on_topic_type_changed(self): + topic = self._widget.topic_line_edit.text() + self._update_topic_type_timer.stop() + if topic == '': + return + try: + if self._use_stamped: + self._publisher_stamped = self._node.create_publisher( + TwistStamped, topic, qos_profile=QoSProfile(depth=10)) + else: + self._publisher = self._node.create_publisher( + Twist, topic, qos_profile=QoSProfile(depth=10)) + except Exception as e: + print('Error creating publisher: %s' % e) + def _on_stop_pressed(self): # If the current value of sliders is zero directly send stop twist msg @@ -285,12 +322,13 @@ def _send_twist(self, x_linear, z_angular): if x_linear == 0.0 and z_angular == 0.0: if not self.zero_cmd_sent: self.zero_cmd_sent = True - self._publisher.publish(twist) - self._publisher_stamped.publish(twist_stamped) else: self.zero_cmd_sent = False - self._publisher.publish(twist) + + if self._use_stamped: self._publisher_stamped.publish(twist_stamped) + else: + self._publisher.publish(twist) def _unregister_publisher(self): if self._publisher is not None: @@ -302,11 +340,14 @@ def _unregister_publisher(self): def shutdown_plugin(self): self._update_parameter_timer.stop() + self._update_topic_type_timer.stop() self._unregister_publisher() def save_settings(self, plugin_settings, instance_settings): instance_settings.set_value( 'topic', self._widget.topic_line_edit.text()) + instance_settings.set_value( + 'stamped', self._widget.stamped_check_box.isChecked()) instance_settings.set_value( 'vx_max', self._widget.max_x_linear_double_spin_box.value()) instance_settings.set_value( @@ -320,6 +361,11 @@ def restore_settings(self, plugin_settings, instance_settings): value = instance_settings.value('topic', '/cmd_vel') value = self._node.get_parameter_or('~default_topic', value) self._widget.topic_line_edit.setText(value) + + value = self._widget.stamped_check_box.isChecked() + value = instance_settings.value('stamped', value) + value = self._node.get_parameter_or('~default_stamped', value) + self._widget.stamped_check_box.setChecked(value == 'true' or value == 'True') value = self._widget.max_x_linear_double_spin_box.value() value = instance_settings.value('vx_max', value)