first commit

rqt_svo/CMakeLists.txt

@@ -0,0 +1,22 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(rqt_svo)
+
+find_package(catkin REQUIRED COMPONENTS
+  rospy
+  rqt_gui
+  rqt_gui_py
+)
+catkin_package()
+catkin_python_setup()
+
+install(FILES plugin.xml
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+install(DIRECTORY resource
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+install(PROGRAMS scripts/rqt_svo
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)

rqt_svo/README.md

@@ -0,0 +1,10 @@
+run with:
+
+    rosrun rqt_svo rqt_svo
+
+or start `rqt` and display the widget.
+
+If it does not work, try this:
+
+    rm ~/.config/ros.org/rqt_gui.ini
+    rqt

rqt_svo/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<package>
+  <name>rqt_svo</name>
+  <version>0.0.0</version>
+  <description>The rqt_svo package</description>
+
+  <maintainer email="[email protected]">cforster</maintainer>
+
+
+  <license>TODO</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>rqt_gui</build_depend>
+  <build_depend>rqt_gui_py</build_depend>
+  <run_depend>rospy</run_depend>
+  <run_depend>rqt_gui</run_depend>
+  <run_depend>rqt_gui_py</run_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <rqt_gui plugin="${prefix}/plugin.xml"/>
+  </export>
+</package>

rqt_svo/plugin.xml

@@ -0,0 +1,12 @@
+<library path="src">
+  <class name="Svo" type="rqt_svo.svo.Svo" base_class_type="rqt_gui_py::Plugin">
+    <description>
+      Plugin for SVO - Semi-Direct Visual Odometry
+    </description>
+    <qtgui>
+      <label>Svo</label>
+      <icon type="theme">system-help</icon>
+      <statustip>Great user interface to provide real value.</statustip>
+    </qtgui>
+  </class>
+</library>

rqt_svo/resource/widget.ui

@@ -0,0 +1,101 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>SvoWidget</class>
+ <widget class="QWidget" name="SvoWidget">
+  <property name="geometry">
+   <rect>
+    <x>0</x>
+    <y>0</y>
+    <width>467</width>
+    <height>140</height>
+   </rect>
+  </property>
+  <property name="windowTitle">
+   <string>SVO</string>
+  </property>
+  <layout class="QVBoxLayout" name="verticalLayout" stretch="0,0,0,0">
+   <item>
+    <layout class="QHBoxLayout" name="horizontalLayout">
+     <item>
+      <widget class="QLabel" name="label">
+       <property name="text">
+        <string>SVO Namespace:</string>
+       </property>
+      </widget>
+     </item>
+     <item>
+      <widget class="QLineEdit" name="topic_line_edit">
+       <property name="toolTip">
+        <string>Topic to send Twist message to</string>
+       </property>
+       <property name="text">
+        <string/>
+       </property>
+      </widget>
+     </item>
+    </layout>
+   </item>
+   <item>
+    <layout class="QHBoxLayout" name="horizontalLayout_4" stretch="0">
+     <item>
+      <widget class="QLabel" name="svo_info_label">
+       <property name="toolTip">
+        <string>Current angular velocity</string>
+       </property>
+       <property name="text">
+        <string>Status Message</string>
+       </property>
+      </widget>
+     </item>
+    </layout>
+   </item>
+   <item>
+    <layout class="QHBoxLayout" name="horizontalLayout_2">
+     <item>
+      <widget class="QProgressBar" name="num_tracked_bar">
+       <property name="maximum">
+        <number>100</number>
+       </property>
+       <property name="value">
+        <number>100</number>
+       </property>
+       <property name="format">
+        <string>%p%</string>
+       </property>
+      </widget>
+     </item>
+    </layout>
+   </item>
+   <item>
+    <layout class="QHBoxLayout" name="horizontalLayout_3">
+     <item>
+      <widget class="QPushButton" name="button_start">
+       <property name="text">
+        <string>Start</string>
+       </property>
+      </widget>
+     </item>
+     <item>
+      <widget class="QPushButton" name="button_reset">
+       <property name="toolTip">
+        <string>Reset velocities to zero</string>
+       </property>
+       <property name="text">
+        <string>Reset</string>
+       </property>
+      </widget>
+     </item>
+     <item>
+      <widget class="QPushButton" name="button_quit">
+       <property name="text">
+        <string>Quit</string>
+       </property>
+      </widget>
+     </item>
+    </layout>
+   </item>
+  </layout>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>

rqt_svo/scripts/rqt_svo

@@ -0,0 +1,10 @@
+#!/usr/bin/env python
+
+import sys
+
+from rqt_svo.svo import Svo
+from rqt_gui.main import Main
+
+plugin = 'rqt_svo'
+main = Main(filename=plugin)
+sys.exit(main.main(sys.argv, standalone=plugin)) # , plugin_argument_provider=Svo.add_arguments

rqt_svo/setup.py

@@ -0,0 +1,12 @@
+#!/usr/bin/env python
+
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+d = generate_distutils_setup(
+    packages=['rqt_svo'],
+    package_dir={'': 'src'},
+    scripts=['scripts/rqt_svo']
+)
+
+setup(**d)

rqt_svo/src/rqt_svo/.gitignore

@@ -0,0 +1 @@
+*.pyc

rqt_svo/src/rqt_svo/svo.py

@@ -0,0 +1,66 @@
+#!/usr/bin/env python
+import os
+import rospy
+import argparse
+from qt_gui.plugin import Plugin
+from .svo_widget import SvoWidget
+
+class Svo(Plugin):
+  """
+  Subclass of Plugin to display SVO status
+  """
+  def __init__(self, context):
+    
+    # Init Plugin
+    super(Svo, self).__init__(context)
+    self.setObjectName('SvoPlugin')
+
+    # Load arguments
+    # TODO load topic name from args
+    args = self._parse_args(context.argv())
+
+    # Create QWidget
+    self._widget = SvoWidget()
+    
+    # Show _widget.windowTitle on left-top of each plugin (when 
+    # it's set in _widget). This is useful when you open multiple 
+    # plugins at once. Also if you open multiple instances of your 
+    # plugin at once, these lines add number to make it easy to 
+    # tell from pane to pane.
+    if context.serial_number() > 1:
+        self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
+    
+    # Add widget to the user interface
+    context.add_widget(self._widget)
+    
+  def _parse_args(self, argv):
+    parser = argparse.ArgumentParser(prog='rqt_svo', add_help=False)
+    group = parser.add_argument_group('Options for rqt_svo plugin')
+    group.add_argument('topic', type=argparse.FileType('r'), nargs='*', default=[], help='Svo Info Topic to display')
+    return parser.parse_args(argv)
+
+#   def shutdown_plugin(self):
+#     # self._unregister_publisher()
+#     pass
+# 
+#   def save_settings(self, plugin_settings, instance_settings):
+#     # TODO save intrinsic configuration, usually using:
+#     # instance_settings.set_value(k, v)
+#     pass
+# 
+#   def restore_settings(self, plugin_settings, instance_settings):
+#     # TODO restore intrinsic configuration, usually using:
+#     # v = instance_settings.value(k)
+#     pass
+
+
+#     def _unregister_publisher(self):
+#         if self._publisher is not None:
+#             self._publisher.unregister()
+#             self._publisher = None
+
+
+  #def trigger_configuration(self):
+      # Comment in to signal that the plugin has a way to configure
+      # This will enable a setting button (gear icon) in each dock widget title bar
+      # Usually used to open a modal configuration dialog

rqt_svo/src/rqt_svo/svo_widget.py

@@ -0,0 +1,126 @@
+#!/usr/bin/env python
+import os
+import rospy
+import rospkg
+import numpy as np
+from python_qt_binding import loadUi
+from python_qt_binding.QtGui import QWidget
+from python_qt_binding.QtCore import QTimer, Slot
+from svo_msgs.msg import Info
+from std_msgs.msg import String
+
+class SvoWidget(QWidget):
+  _last_info_msg = Info()
+  _publisher = None
+  _subscriber = None
+  _num_received_msgs = 0
+  
+  def __init__(self, svo_namespace='svo'):
+    
+    # Init QWidget
+    super(SvoWidget, self).__init__()
+    self.setObjectName('SvoWidget')
+    
+    # load UI
+    ui_file = os.path.join(rospkg.RosPack().get_path('rqt_svo'), 'resource', 'widget.ui')
+    loadUi(ui_file, self)
+     
+    # init and start update timer for data, the timer calls the function update_info all 40ms    
+    self._update_info_timer = QTimer(self)
+    self._update_info_timer.timeout.connect(self.update_info)
+    self._update_info_timer.start(40)
+    
+    # set the functions that are called when a button is pressed
+    self.button_start.pressed.connect(self.on_start_button_pressed)
+    self.button_reset.pressed.connect(self.on_reset_button_pressed)
+    self.button_quit.pressed.connect(self.on_quit_button_pressed)
+    
+    # set callback for changed topic
+    self.topic_line_edit.setText(svo_namespace)
+    self.register(svo_namespace)
+    self.topic_line_edit.textChanged.connect(self._on_topic_changed)
+      
+    # TODO: set a timer when the last message was received and give a warning if it is too long ago!
+    
+  @Slot(str)
+  def _on_topic_changed(self, topic):
+    topic = str(topic)
+    self.unregister()
+    self.register(topic)
+        
+  def register(self, svo_namespace):
+    # Load parameters
+    max_num_features = rospy.get_param(svo_namespace+'/max_fts', 120)
+    
+    # Feature bar
+    self.num_tracked_bar.setMaximum(max_num_features)
+    print('set maximum number of features to '+str(max_num_features))
+    
+    # Subscribe to ROS Info topic and register callback
+    self._subscriber = rospy.Subscriber(svo_namespace+'/info', Info, self.info_cb)
+    
+    # Initialize Publisher
+    self._publisher = rospy.Publisher(svo_namespace+'/remote_key', String)
+  
+  def unregister(self):
+    if self._publisher is not None:
+      self._publisher.unregister()
+      self._publisher = None
+      
+    if self._subscriber is not None:
+      self._subscriber.unregister()
+      self._subscriber = None
+  
+  
+  def info_cb(self, msg):
+    self._last_info_msg = msg
+    self._num_received_msgs += 1
+    
+  def update_info(self):
+    info_text = 'Not Connected'
+    if self._num_received_msgs > 0:
+      fps = 0
+      if self._last_info_msg.processing_time > 0:
+        fps = 1.0/self._last_info_msg.processing_time
+      info_text = 'fps = %.2f' % fps
+      info_text += '\t #Features = ' + str(self._last_info_msg.num_matches)
+      info_text += '\t'
+      if self._last_info_msg.stage == 0:
+        info_text += '\t PAUSED'
+      elif self._last_info_msg.stage == 1:
+        info_text += '\t FIRST_FRAME'
+      elif self._last_info_msg.stage == 2:
+        info_text += '\t SECOND_FRAME'
+      elif self._last_info_msg.stage == 3:
+        info_text += '\t RUNNING'
+      if self._last_info_msg.tracking_quality == 0:
+        info_text += '\t CRITICAL'
+      elif self._last_info_msg.tracking_quality == 1:
+        info_text += '\t BAD TRACKING'
+      elif self._last_info_msg.tracking_quality == 2:
+        info_text += '\t GOOD TRACKING'
+
+    # set info text
+    self.svo_info_label.setText(info_text)
+    
+    # set progress bar
+    self.num_tracked_bar.setValue(self._last_info_msg.num_matches)
+    
+  def on_start_button_pressed(self):
+    print('START SLAM')
+    self.send_command('s')
+    
+  def on_reset_button_pressed(self):
+    print('RESET SLAM')
+    self.send_command('r')
+    
+  def on_quit_button_pressed(self):
+    print('QUIT SLAM')
+    self.send_command('q')
+    
+  def send_command(self, cmd):
+    if self._publisher is None:
+      return
+    self._publisher.publish(String(cmd))
+    
+