Add rudamentary message type selection

package.xml

@@ -14,6 +14,7 @@
   <author>Dirk Thomas</author>
 
   <exec_depend>ament_index_python</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
   <exec_depend>geometry_msgs</exec_depend>
   <exec_depend version_gte="0.2.19">python_qt_binding</exec_depend>
   <exec_depend>rclpy</exec_depend>

resource/RobotSteering.ui

@@ -14,6 +14,13 @@
    <string>Robot Steering</string>
   </property>
   <layout class="QVBoxLayout" name="verticalLayout" stretch="0,0,0,0,0">
+   <item>
+    <widget class="QComboBox" name="msgtype_combo_box">
+     <property name="toolTip">
+      <string>Message type selection</string>
+     </property>
+    </widget>
+   </item>
    <item>
     <layout class="QHBoxLayout" name="horizontalLayout">
      <item>

src/rqt_robot_steering/robot_steering.py

@@ -31,15 +31,15 @@
 import os
 
 from ament_index_python import get_resource
-from geometry_msgs.msg import Twist
+from std_msgs.msg import Header
+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.QtCore import Qt, QTimer, Slot, QThread, QStringListModel, QModelIndex
 from python_qt_binding.QtGui import QKeySequence
-from python_qt_binding.QtWidgets import QShortcut, QWidget
+from python_qt_binding.QtWidgets import QShortcut, QWidget, QCompleter
 from rclpy.qos import QoSProfile
 from rqt_gui_py.plugin import Plugin
 
-
 class RobotSteering(Plugin):
 
     slider_factor = 1000.0
@@ -50,6 +50,7 @@ def __init__(self, context):
 
         self._node = context.node
 
+        self._msgtype = ""
         self._publisher = None
 
         self._widget = QWidget()
@@ -62,6 +63,15 @@ def __init__(self, context):
                 self._widget.windowTitle() + (' (%d)' % context.serial_number()))
         context.add_widget(self._widget)
 
+        self._widget.msgtype_combo_box.insertItem(0, "Twist")
+        self._widget.msgtype_combo_box.insertItem(1, "TwistStamped")
+
+        self._widget.msgtype_combo_box.currentTextChanged.connect(
+            self._on_msgtype_changed)
+
+        # Run the callback once so _msgtype is initialized
+        self._on_msgtype_changed()
+
         self._widget.topic_line_edit.textChanged.connect(
             self._on_topic_changed)
         self._widget.stop_push_button.pressed.connect(self._on_stop_pressed)
@@ -167,20 +177,63 @@ def __init__(self, context):
         self._widget.decrease_z_angular_push_button.setToolTip(
             self._widget.decrease_z_angular_push_button.toolTip() + ' ' + self.tr('([Shift +] D)'))
 
-        # timer to consecutively send twist messages
+        # Timer to consecutively send twist messages
         self._update_parameter_timer = QTimer(self)
         self._update_parameter_timer.timeout.connect(
             self._on_parameter_changed)
         self._update_parameter_timer.start(100)
         self.zero_cmd_sent = False
 
+        # Twist/TwistStamped topic name autocomplete
+        self.topic_completer = QCompleter()
+        self.topic_completer.setCompletionMode(QCompleter.PopupCompletion)
+        self._widget.topic_line_edit.setCompleter(self.topic_completer)
+        self.model = QStringListModel()
+        self.topic_completer.setModel(self.model)
+
+        self._on_topic_refresh_timer()
+
+        # Timer to periodically update topic suggestions
+        self._update_topic_completer_timer = QTimer(self)
+        self._update_topic_completer_timer.timeout.connect(
+            self._on_topic_refresh_timer)
+        self._update_topic_completer_timer.start(10000)
+
+    def _on_topic_refresh_timer(self):
+        topics_and_names = self._node.get_topic_names_and_types()
+        relevant_topics = []
+
+        for name, topic in topics_and_names:
+            if 'geometry_msgs/msg/Twist' in topic or 'geometry_msgs/msg/TwistStamped' in topic:
+                relevant_topics.append(name)
+
+        self.model = QStringListModel(relevant_topics)
+        self.topic_completer.setModel(self.model)
+
+
+    def _on_msgtype_changed(self):
+        # TODO
+        self._msgtype = self._widget.msgtype_combo_box.currentText()
+        self._on_topic_changed(self._widget.topic_line_edit.text())
+
     @Slot(str)
     def _on_topic_changed(self, topic):
+
         topic = str(topic)
         self._unregister_publisher()
-        if topic == '':
+
+        # TODO: Is there a check_topic_valid()?
+        if topic == '' or not topic.startswith("/") or topic.endswith("/"):
+            # TODO: Set qlineedits's background color red while this is true?
+            return
+
+        if (self._msgtype == "Twist"):
+            self._publisher = self._node.create_publisher(Twist, topic, qos_profile=QoSProfile(depth=10))
+        elif (self._msgtype == "TwistStamped"):
+            self._publisher = self._node.create_publisher(TwistStamped, topic, qos_profile=QoSProfile(depth=10))
+        else:
+            self._publisher = None
             return
-        self._publisher = self._node.create_publisher(Twist, topic, qos_profile=QoSProfile(depth=10))
 
     def _on_stop_pressed(self):
         # If the current value of sliders is zero directly send stop twist msg
@@ -264,6 +317,7 @@ def _on_parameter_changed(self):
     def _send_twist(self, x_linear, z_angular):
         if self._publisher is None:
             return
+
         twist = Twist()
         twist.linear.x = x_linear
         twist.linear.y = 0.0
@@ -272,20 +326,34 @@ def _send_twist(self, x_linear, z_angular):
         twist.angular.y = 0.0
         twist.angular.z = z_angular
 
+        if self._msgtype == "Twist":
+            twist_msg = twist
+        elif self._msgtype == "TwistStamped":
+            twist_msg = TwistStamped()
+            twist_msg.header = Header()
+            twist_msg.header.stamp = self._node.get_clock().now().to_msg()
+            twist_msg.twist = twist
+        else:
+            return
+
         # 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.publish(twist_msg)
         else:
             self.zero_cmd_sent = False
-            self._publisher.publish(twist)
+            self._publisher.publish(twist_msg)
 
     def _unregister_publisher(self):
         if self._publisher is not None:
             self._node.destroy_publisher(self._publisher)
             self._publisher = None
 
+            # A short delay between stopping the publisher is needed
+            # if we want to create a new one on the same topic with a different message type
+            QThread.msleep(100)
+
     def shutdown_plugin(self):
         self._update_parameter_timer.stop()
         self._unregister_publisher()