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.

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

Porting to ros2 #17

Merged
merged 1 commit into from
Dec 10, 2018
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
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
20 changes: 12 additions & 8 deletions CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -1,18 +1,22 @@
cmake_minimum_required(VERSION 2.8.3)
cmake_minimum_required(VERSION 3.5)
project(rqt_plot)
# Load catkin and all dependencies required for this package
find_package(catkin REQUIRED)
catkin_package()
catkin_python_setup()

find_package(ament_cmake REQUIRED)
find_package(ament_cmake_python REQUIRED)

ament_python_install_package(${PROJECT_NAME}
PACKAGE_DIR src/${PROJECT_NAME})

install(FILES plugin.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
DESTINATION share/${PROJECT_NAME}
)

install(DIRECTORY resource
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
DESTINATION share/${PROJECT_NAME}
)

install(PROGRAMS scripts/rqt_plot
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
DESTINATION lib/${PROJECT_NAME}
)

ament_package()
29 changes: 14 additions & 15 deletions package.xml
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
<package>
<package format="2">
<name>rqt_plot</name>
<version>0.4.8</version>
<description>rqt_plot provides a GUI plugin visualizing numeric values in a 2D plot using different plotting backends.</description>
Expand All @@ -14,23 +14,22 @@

<author>Dorian Scholz</author>

<buildtool_depend>catkin</buildtool_depend>
<buildtool_depend>ament_cmake</buildtool_depend>

<run_depend>python-matplotlib</run_depend>
<run_depend>python-numpy</run_depend>
<run_depend>python-rospkg</run_depend>
<run_depend version_gte="0.2.19">python_qt_binding</run_depend>
<run_depend version_gte="0.2.25">qt_gui_py_common</run_depend>
<run_depend>qwt_dependency</run_depend>
<run_depend>rosgraph</run_depend>
<run_depend>rostopic</run_depend>
<run_depend>rqt_gui</run_depend>
<run_depend>rqt_gui_py</run_depend>
<run_depend>rqt_py_common</run_depend>
<run_depend>std_msgs</run_depend>
<exec_depend version_gte="0.2.19">python_qt_binding</exec_depend>
<exec_depend>python3-matplotlib</exec_depend>
<exec_depend>python3-numpy</exec_depend>
<exec_depend>python3-catkin-pkg-modules</exec_depend>
<exec_depend version_gte="0.2.25">qt_gui_py_common</exec_depend>
<exec_depend>qwt_dependency</exec_depend>
<exec_depend>rclpy</exec_depend>
<exec_depend>rqt_gui</exec_depend>
<exec_depend>rqt_gui_py</exec_depend>
<exec_depend>rqt_py_common</exec_depend>
<exec_depend>std_msgs</exec_depend>

<export>
<architecture_independent/>
<build_type>ament_cmake</build_type>
<rqt_gui plugin="${prefix}/plugin.xml"/>
</export>
</package>
2 changes: 1 addition & 1 deletion scripts/rqt_plot
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
#!/usr/bin/env python
#!/usr/bin/env python3

import sys

Expand Down
45 changes: 36 additions & 9 deletions src/rqt_plot/plot.py
Original file line number Diff line number Diff line change
Expand Up @@ -50,10 +50,10 @@ def __init__(self, context):
self.setObjectName('Plot')

self._context = context

self._node = context.node
self._args = self._parse_args(context.argv())
self._widget = PlotWidget(
initial_topics=self._args.topics, start_paused=self._args.start_paused)
self._node, initial_topics=self._args.topics, start_paused=self._args.start_paused)
self._data_plot = DataPlot(self._widget)

# disable autoscaling of X, and set a sane default range
Expand All @@ -67,6 +67,34 @@ def __init__(self, context):
self._widget.windowTitle() + (' (%d)' % context.serial_number()))
context.add_widget(self._widget)

def _resolve_topic_name(script_name, name):
"""
Name resolver for scripts.

Derived from
https://github.com/ros/ros_comm/blob/melodic-devel/tools/rosgraph/src/rosgraph/names.py

:param name: name to resolve, ``str``
:param script_name: name of script. script_name must not
contain a namespace., ``str``
:returns: resolved name, ``str``
"""
sep = '/'
priv_name = '~'

# empty string resolves to namespace
if not name:
return self._node.get_namespace()
# Check for global name: /foo/name resolves to /foo/name
if name[0] == sep:
return name
# Check for private name: ~name resolves to /caller_id/name
elif name[0] == priv_name:
if script_name[-1] == sep:
return script_name + name[1:]
return script_name + sep + name[1:]
return self._node.get_namespace() + name

def _parse_args(self, argv):
parser = argparse.ArgumentParser(prog='rqt_plot', add_help=False)
Plot.add_arguments(parser)
Expand All @@ -84,7 +112,7 @@ def _parse_args(self, argv):
base = sub_t[:sub_t.find(':')]
# the first prefix includes a field name, so save then strip it off
c_topics.append(base)
if not '/' in base:
if '/' not in base:
parser.error("%s must contain a topic and field name" % sub_t)
base = base[:base.rfind('/')]

Expand All @@ -94,12 +122,11 @@ def _parse_args(self, argv):
else:
c_topics.append(sub_t)
# 1053: resolve command-line topic names
import rosgraph
c_topics = [rosgraph.names.script_resolve_name('rqt_plot', n) for n in c_topics]
if type(c_topics) == list:
topic_list.extend(c_topics)
else:
topic_list.append(c_topics)
topics, topic_types = self._node.get_topic_names_and_types()

c_topics = [_resolve_topic_name('rqt_plot', n) for n in c_topics]
topic_list.extend(c_topics)

args.topics = topic_list

return args
Expand Down
110 changes: 75 additions & 35 deletions src/rqt_plot/plot_widget.py
Original file line number Diff line number Diff line change
Expand Up @@ -31,47 +31,87 @@
# POSSIBILITY OF SUCH DAMAGE.

import os
import rospkg
import roslib
import time

from ament_index_python.resources import get_resource
from python_qt_binding import loadUi
from python_qt_binding.QtCore import Qt, QTimer, qWarning, Slot
from python_qt_binding.QtGui import QIcon
from python_qt_binding.QtWidgets import QAction, QMenu, QWidget

import rospy

from rqt_py_common.topic_completer import TopicCompleter
from rqt_py_common import topic_helpers

from . rosplot import ROSData, RosPlotException
from rqt_py_common import topic_helpers, message_helpers

from rqt_plot.rosplot import ROSData, RosPlotException


def _parse_type(topic_type_str):
slot_type = topic_type_str
is_array = False
array_size = None

array_idx = topic_type_str.find('[')
if array_idx < 0:
return slot_type, False, None

end_array_idx = topic_type_str.find(']', start=array_idx + 1)
if end_array_idx < 0:
return None, False, None

slot_type = topic_type_str[:array_idx]
array_size_str = topic_type_str[array_idx + 1, end_array_idx]
try:
array_size = int(array_size_str)
return slot_type, True, array_size
except ValueError as e:
return slot_type, True, None


def get_plot_fields(node, topic_name):
topics = node.get_topic_names_and_types()
real_topic = None
for name, topic_types in topics:
if name == topic_name[:len(name)]:
real_topic = name
topic_type_str = topic_types[0] if topic_types else None
break
if real_topic is None:
message = "topic %s does not exist" % (topic_name)
return [], message

if topic_type_str is None:
message = "no topic types found for topic %s " % (topic_name)
return [], message

def get_plot_fields(topic_name):
topic_type, real_topic, _ = topic_helpers.get_topic_type(topic_name)
if topic_type is None:
message = "topic %s does not exist" % (topic_name)
if len(topic_name) < len(real_topic) + 1:
message = 'no field specified in topic name "{}"'.format(topic_name)
return [], message

field_name = topic_name[len(real_topic) + 1:]

slot_type, is_array, array_size = roslib.msgs.parse_type(topic_type)
field_class = roslib.message.get_message_class(slot_type)
message_class = message_helpers.get_message_class(topic_type_str)
if message_class is None:
message = 'message class "{}" is invalid'.format(topic_type_str)
return [], message

slot_type, is_array, array_size = _parse_type(topic_type_str)
field_class = message_helpers.get_message_class(slot_type)

fields = [f for f in field_name.split('/') if f]

for field in fields:
# parse the field name for an array index
try:
field, _, field_index = roslib.msgs.parse_type(field)
except roslib.msgs.MsgSpecException:
field, _, field_index = _parse_type(field)
if field is None:
message = "invalid field %s in topic %s" % (field, real_topic)
return [], message

if field not in getattr(field_class, '__slots__', []):
field_names_and_types = field_class.get_fields_and_field_types()
if field not in field_names_and_types:
message = "no field %s in topic %s" % (field_name, real_topic)
return [], message
slot_type = field_class._slot_types[field_class.__slots__.index(field)]
slot_type, slot_is_array, array_size = roslib.msgs.parse_type(slot_type)
slot_type = field_names_and_types[field]
slot_type, slot_is_array, array_size = _parse_type(slot_type)
is_array = slot_is_array and field_index is None

field_class = topic_helpers.get_type_class(slot_type)
Expand All @@ -89,11 +129,10 @@ def get_plot_fields(topic_name):
message = "topic %s is %s" % (topic_name, topic_kind)
return [topic_name], message
else:
if not roslib.msgs.is_valid_constant_type(slot_type):
if not topic_helpers.is_primitive_type(slot_type):
numeric_fields = []
for i, slot in enumerate(field_class.__slots__):
slot_type = field_class._slot_types[i]
slot_type, is_array, array_size = roslib.msgs.parse_type(slot_type)
for slot, slot_type in field_class.get_fields_and_field_types().items():
slot_type, is_array, array_size = _parse_type(slot_type)
slot_class = topic_helpers.get_type_class(slot_type)
if slot_class in (int, float) and not is_array:
numeric_fields.append(slot)
Expand All @@ -108,22 +147,23 @@ def get_plot_fields(topic_name):
return [], message


def is_plottable(topic_name):
fields, message = get_plot_fields(topic_name)
def is_plottable(node, topic_name):
fields, message = get_plot_fields(node, topic_name)
return len(fields) > 0, message


class PlotWidget(QWidget):
_redraw_interval = 40

def __init__(self, initial_topics=None, start_paused=False):
def __init__(self, node, initial_topics=None, start_paused=False):
super(PlotWidget, self).__init__()
self.setObjectName('PlotWidget')

self._node = node
self._initial_topics = initial_topics

rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_plot'), 'resource', 'plot.ui')
_, package_path = get_resource('packages', 'rqt_plot')
ui_file = os.path.join(package_path, 'share', 'rqt_plot', 'resource', 'plot.ui')
loadUi(ui_file, self)
self.subscribe_topic_button.setIcon(QIcon.fromTheme('list-add'))
self.remove_topic_button.setIcon(QIcon.fromTheme('list-remove'))
Expand All @@ -136,10 +176,10 @@ def __init__(self, initial_topics=None, start_paused=False):
self.pause_button.setChecked(True)

self._topic_completer = TopicCompleter(self.topic_edit)
self._topic_completer.update_topics()
self._topic_completer.update_topics(node)
self.topic_edit.setCompleter(self._topic_completer)

self._start_time = rospy.get_time()
self._start_time = time.time()
self._rosdata = {}
self._remove_topic_menu = QMenu()

Expand Down Expand Up @@ -192,7 +232,7 @@ def dragEnterEvent(self, event):
topic_name = str(event.mimeData().text())

# check for plottable field type
plottable, message = is_plottable(topic_name)
plottable, message = is_plottable(self._node, topic_name)
if plottable:
event.acceptProposedAction()
else:
Expand All @@ -211,9 +251,9 @@ def dropEvent(self, event):
def on_topic_edit_textChanged(self, topic_name):
# on empty topic name, update topics
if topic_name in ('', '/'):
self._topic_completer.update_topics()
self._topic_completer.update_topics(self._node)

plottable, message = is_plottable(topic_name)
plottable, message = is_plottable(self._node, topic_name)
self.subscribe_topic_button.setEnabled(plottable)
self.subscribe_topic_button.setToolTip(message)

Expand Down Expand Up @@ -280,11 +320,11 @@ def make_remove_topic_function(x):

def add_topic(self, topic_name):
topics_changed = False
for topic_name in get_plot_fields(topic_name)[0]:
for topic_name in get_plot_fields(self._node, topic_name)[0]:
if topic_name in self._rosdata:
qWarning('PlotWidget.add_topic(): topic already subscribed: %s' % topic_name)
continue
self._rosdata[topic_name] = ROSData(topic_name, self._start_time)
self._rosdata[topic_name] = ROSData(self._node, topic_name, self._start_time)
if self._rosdata[topic_name].error is not None:
qWarning(str(self._rosdata[topic_name].error))
del self._rosdata[topic_name]
Expand Down
Loading