Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Publish TwistStamped message optionally #18

Open
wants to merge 2 commits into
base: dashing-devel
Choose a base branch
from
Open

Publish TwistStamped message optionally #18

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
ad65f93
Publish TwistStamped message too
ahcorde Jul 20, 2023
1be4a11
Add a stamped checkbox and publish only one topic
christophfroehlich Mar 23, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 10 additions & 0 deletions resource/RobotSteering.ui
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,16 @@
</property>
</widget>
</item>
<item>
<widget class="QCheckBox" name="stamped_check_box">
<property name="text">
<string>stamped</string>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
<item>
<widget class="QPushButton" name="stop_push_button">
<property name="toolTip">
Expand Down
70 changes: 65 additions & 5 deletions src/rqt_robot_steering/robot_steering.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -51,10 +51,13 @@ def __init__(self, context):
self._node = context.node

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:
Expand All @@ -64,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(
Expand Down Expand Up @@ -180,7 +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))
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
Expand Down Expand Up @@ -262,8 +302,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
Expand All @@ -272,27 +313,41 @@ 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)
else:
self.zero_cmd_sent = False

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:
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()
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(
Expand All @@ -306,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)
Expand Down