class ModelLogger:
"""
Convenience class for logging data.
"""
def __init__(self):
"""
Initializes the ModelLogger.
"""
self.__log_model = QStandardItemModel(0, 4, None)
self.__log_model.setHorizontalHeaderLabels(
["type", "date", "location", "message"])
def log(self, type, date, location, message):
"""
Adds a log entry to the log_model.
:param type: the type of the log_entry
:type type: str
:param date: the time of the log_entry
:type date: Time
:param location: the location, were the fault/info/... occured
:type location: str
:param message: the message of the log_entry
:type message: str
"""
self.__log_model.insertRow(0)
self.__log_model.setData(self.__log_model.index(0, 0), str(type))
self.__log_model.setData(
self.__log_model.index(0, 1),
time.strftime("%d.%m-%H:%M:%S",
time.localtime(int(str(date)) / 1000000000)))
self.__log_model.setData(self.__log_model.index(0, 2), str(location))
self.__log_model.setData(self.__log_model.index(0, 3), str(message))
def get_representation(self):
"""
Returns the log as a QStandartItemModel
:returns: the log-model
:rtype: QStandartItemModel
"""
return self.__log_model
class ModelLogger:
"""
Convenience class for logging data.
"""
def __init__(self):
"""
Initializes the ModelLogger.
"""
self.__log_model = QStandardItemModel(0, 4, None)
self.__log_model.setHorizontalHeaderLabels(["type", "date", "location", "message"])
def log(self, type, date, location, message):
"""
Adds a log entry to the log_model.
:param type: the type of the log_entry
:type type: str
:param date: the time of the log_entry
:type date: Time
:param location: the location, were the fault/info/... occured
:type location: str
:param message: the message of the log_entry
:type message: str
"""
self.__log_model.insertRow(0)
self.__log_model.setData(self.__log_model.index(0, 0), str(type))
self.__log_model.setData(self.__log_model.index(0, 1), time.strftime("%d.%m-%H:%M:%S", time.localtime(int(str(date)) / 1000000000)))
self.__log_model.setData(self.__log_model.index(0, 2), str(location))
self.__log_model.setData(self.__log_model.index(0, 3), str(message))
def get_representation(self):
"""
Returns the log as a QStandartItemModel
:returns: the log-model
:rtype: QStandartItemModel
"""
return self.__log_model
class MoveitWidget(QWidget):
"""#TODO: comment
"""
# To be connected to PluginContainerWidget
sig_sysmsg = None
sig_param = Signal(bool, str) # param name emitted
sig_node = Signal(bool, str) # param name emitted
_SPLITTER_H = 'splitter_horizontal'
def __init__(self, parent, plugin_context):
"""
@type parent: MoveitMain
"""
self._nodes_monitored = ['/move_group']
self._selected_topics = [('/pointcloud', 'sensor_msgs/PointCloud'),
('/pointcloud2', 'sensor_msgs/PointCloud2'),
('/image', 'sensor_msgs/Image'),
('/camera_info', 'sensor_msgs/CameraInfo')]
self._params_monitored = [
'/robot_description', '/robot_description_semantic'
]
super(MoveitWidget, self).__init__()
self._parent = parent
self._plugin_context = plugin_context
self._refresh_rate = 5 # With default value
self._rospack = rospkg.RosPack()
ui_file = os.path.join(self._rospack.get_path('rqt_moveit'),
'resource', 'moveit_top.ui')
loadUi(ui_file, self, {'TopicWidget': TopicWidget})
# Custom widget classes don't show in QSplitter when they instantiated
# in .ui file and not explicitly added to QSplitter like this. Thus
# this is a workaround.
self._splitter.addWidget(self._widget_topic)
self._spinbox_refreshrate.valueChanged.connect(
self._update_refreshrate)
# Show default ref rate on QSpinbox
self._spinbox_refreshrate.setValue(self._refresh_rate)
# Monitor node
self._is_checking_nodes = True
self._node_qitems = {}
self._node_monitor_thread = self._init_monitor_nodes(
self._nodes_monitored)
self._node_monitor_thread.start()
#TODO: connect sys msg for nodes.
# topic to show
# Delegate GUI functionality to rqt_topic.TopicWidget.
self._widget_topic.set_selected_topics(self._selected_topics)
self._widget_topic.set_topic_specifier(TopicWidget.SELECT_BY_MSGTYPE)
self._widget_topic.start()
# To connect signal in a widget to PluginContainerWidget.
#TODO: In this way, Signal from only one instance is hooked.
# Not a good design at all.
self.sig_sysmsg = self._widget_topic.sig_sysmsg
# Init monitoring parameters.
self._is_checking_params = True
self._param_qitems = {}
_col_names_paramtable = ['Param name', 'Found on Parameter Server?']
self._param_check_thread = self._init_monitor_parameters(
self._params_monitored, _col_names_paramtable)
self._param_check_thread.start()
def _init_monitor_nodes(self, nodes_monitored):
"""
@type params_monitored: str[]
@rtype: Thread
"""
self._node_datamodel = QStandardItemModel(0, 2)
self._root_qitem = self._node_datamodel.invisibleRootItem()
self._view_nodes.setModel(self._node_datamodel)
node_monitor_thread = Thread(target=self._check_nodes_alive,
args=(self.sig_node,
self._nodes_monitored))
# self._node_monitor_thread = NodeMonitorThread(self, self.sig_node,
# nodes_monitored)
self.sig_node.connect(self._update_output_nodes)
return node_monitor_thread
def _check_nodes_alive(self, signal, nodes_monitored):
"""
Working as a callback of Thread class, this method keeps looping to
watch if the nodes whose names are passed exist and emits signal per
each node.
Notice that what MoveitWidget._check_nodes_alive &
MoveitWidget._check_params_alive do is very similar, but since both of
them are supposed to be passed to Thread class, there might not be
a way to generalize these 2.
@param signal: Signal(bool, str)
@type nodes_monitored: str[]
"""
while self._is_checking_nodes:
rosnode_dynamically_loaded = __import__('rosnode')
#from rosnode import rosnode_ping
for nodename in nodes_monitored:
#TODO: rosnode_ping prints when the node is not found.
# Currently I have no idea how to capture that from here.
try:
#is_node_running = rosnode_ping(nodename, 1)
is_node_running = rosnode_dynamically_loaded.rosnode_ping(
nodename, 1)
except rosnode_dynamically_loaded.ROSNodeIOException as e:
#TODO: Needs to be indicated on GUI
# (eg. PluginContainerWidget)
rospy.logerr(e.message)
is_node_running = False
signal.emit(is_node_running, nodename)
rospy.logdebug('_update_output_nodes')
del rosnode_dynamically_loaded
time.sleep(self._refresh_rate)
def _init_monitor_parameters(self,
params_monitored,
_col_names_paramtable=None):
"""
@type params_monitored: str[]
"""
self._params_monitored = params_monitored
self._param_datamodel = QStandardItemModel(0, 2)
self._root_qitem = self._param_datamodel.invisibleRootItem()
self._view_params.setModel(self._param_datamodel)
# Names of header on the QTableView.
if not _col_names_paramtable:
_col_names_paramtable = [
'Param name', 'Found on Parameter Server?'
]
self._param_datamodel.setHorizontalHeaderLabels(_col_names_paramtable)
self.sig_param.connect(self._update_output_parameters)
param_check_thread = Thread(target=self._check_params_alive,
args=(self.sig_param,
self._params_monitored))
return param_check_thread
def _update_output_nodes(self, is_node_running, node_name):
"""
Slot for signals that tell nodes existence.
@type is_node_running: bool
@type node_name: str
"""
#TODO: What this method does is exactly the same with
# monitor_parameters. Unify them.
rospy.logdebug('is_node_running={} par_name={}'.format(
is_node_running, node_name))
node_name = str(node_name)
node_qitem = None
if not node_name in self._node_qitems:
node_qitem = QStandardItem(node_name)
self._node_qitems[node_name] = node_qitem
self._node_datamodel.appendRow(node_qitem)
else: # qsitem with the node name already exists.
node_qitem = self._node_qitems[str(node_name)]
qindex = self._node_datamodel.indexFromItem(node_qitem)
_str_node_running = 'Not running'
if is_node_running:
_str_node_running = 'Running'
qitem_node_status = QStandardItem(_str_node_running)
self._node_datamodel.setItem(qindex.row(), 1, qitem_node_status)
def _check_params_alive(self, signal, params_monitored):
"""
Working as a callback of Thread class, this method keeps looping to
watch if the params whose names are passed exist and emits signal per
each node.
Notice that what MoveitWidget._check_nodes_alive &
MoveitWidget._check_params_alive do is very similar, but since both of
them are supposed to be passed to Thread class, there might not be
a way to generalize these 2.
@type signal: Signal(bool, str)
@param_name signal: emitting a name of the parameter that's found.
@type params_monitored: str[]
"""
while self._is_checking_params:
# self._is_checking_params only turns to false when the plugin
# shuts down.
has_param = False
for param_name in params_monitored:
is_rosmaster_running = RqtRoscommUtil.is_roscore_running()
try:
if is_rosmaster_running:
# Only if rosmaster is running, check if the parameter
# exists or not.
has_param = rospy.has_param(param_name)
except rospy.exceptions.ROSException as e:
self.sig_sysmsg.emit(
'Exception upon rospy.has_param {}'.format(e.message))
signal.emit(has_param, param_name)
rospy.loginfo('has_param {}, check_param_alive: {}'.format(
has_param, param_name))
time.sleep(self._refresh_rate)
def _update_output_parameters(self, has_param, param_name):
"""
Slot
@type has_param: bool
@type param_name: str
"""
rospy.logdebug('has_param={} par_name={}'.format(
has_param, param_name))
param_name = str(param_name)
param_qitem = None
if not param_name in self._param_qitems:
param_qitem = QStandardItem(param_name)
self._param_qitems[param_name] = param_qitem
self._param_datamodel.appendRow(param_qitem)
else: # qsitem with the param name already exists.
param_qitem = self._param_qitems[str(param_name)]
qindex = self._param_datamodel.indexFromItem(param_qitem)
_str_param_found = 'No'
if has_param:
_str_param_found = 'Yes'
qitem_param_status = QStandardItem(_str_param_found)
self._param_datamodel.setItem(qindex.row(), 1, qitem_param_status)
self._view_params.resizeColumnsToContents()
def _update_refreshrate(self, refresh_rate):
"""
Slot
@type refresh_rate: int
"""
self._refresh_rate = refresh_rate
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value(self._SPLITTER_H,
self._splitter.saveState())
def restore_settings(self, plugin_settings, instance_settings):
if instance_settings.contains(self._SPLITTER_H):
self._splitter.restoreState(
instance_settings.value(self._SPLITTER_H))
else:
self._splitter.setSizes([100, 100, 200])
pass
def shutdown(self):
"""
Overridden.
Close threads.
@raise RuntimeError:
"""
try:
#self._node_monitor_thread.join() # No effect
self._is_checking_nodes = False
self._node_monitor_thread = None
#self._param_check_thread.join()
self._is_checking_params = False
self._param_check_thread = None
except RuntimeError as e:
rospy.logerr(e)
raise e
class MoveitWidget(QWidget):
"""#TODO: comment
"""
# To be connected to PluginContainerWidget
sig_sysmsg = None
sig_param = Signal(bool, str) # param name emitted
sig_node = Signal(bool, str) # param name emitted
_SPLITTER_H = 'splitter_horizontal'
def __init__(self, parent, plugin_context):
"""
@type parent: MoveitMain
"""
self._nodes_monitored = ['/move_group']
self._selected_topics = [('/pointcloud', 'sensor_msgs/PointCloud'),
('/pointcloud2', 'sensor_msgs/PointCloud2'),
('/image', 'sensor_msgs/Image'),
('/camera_info', 'sensor_msgs/CameraInfo')]
self._params_monitored = ['/robot_description',
'/robot_description_semantic']
super(MoveitWidget, self).__init__()
self._parent = parent
self._plugin_context = plugin_context
self._refresh_rate = 5 # With default value
self._rospack = rospkg.RosPack()
ui_file = os.path.join(self._rospack.get_path('rqt_moveit'),
'resource', 'moveit_top.ui')
loadUi(ui_file, self, {'TopicWidget': TopicWidget})
# Custom widget classes don't show in QSplitter when they instantiated
# in .ui file and not explicitly added to QSplitter like this. Thus
# this is a workaround.
self._splitter.addWidget(self._widget_topic)
self._spinbox_refreshrate.valueChanged.connect(
self._update_refreshrate)
# Show default ref rate on QSpinbox
self._spinbox_refreshrate.setValue(self._refresh_rate)
# Monitor node
self._is_checking_nodes = True
self._node_qitems = {}
self._node_monitor_thread = self._init_monitor_nodes(
self._nodes_monitored)
self._node_monitor_thread.start()
#TODO: connect sys msg for nodes.
# topic to show
# Delegate GUI functionality to rqt_topic.TopicWidget.
self._widget_topic.set_selected_topics(self._selected_topics)
self._widget_topic.set_topic_specifier(TopicWidget.SELECT_BY_MSGTYPE)
self._widget_topic.start()
# To connect signal in a widget to PluginContainerWidget.
#TODO: In this way, Signal from only one instance is hooked.
# Not a good design at all.
self.sig_sysmsg = self._widget_topic.sig_sysmsg
# Init monitoring parameters.
self._is_checking_params = True
self._param_qitems = {}
_col_names_paramtable = ['Param name', 'Found on Parameter Server?']
self._param_check_thread = self._init_monitor_parameters(
self._params_monitored,
_col_names_paramtable)
self._param_check_thread.start()
def _init_monitor_nodes(self, nodes_monitored):
"""
@type params_monitored: str[]
@rtype: Thread
"""
self._node_datamodel = QStandardItemModel(0, 2)
self._root_qitem = self._node_datamodel.invisibleRootItem()
self._view_nodes.setModel(self._node_datamodel)
node_monitor_thread = Thread(target=self._check_nodes_alive,
args=(self.sig_node,
self._nodes_monitored))
# self._node_monitor_thread = NodeMonitorThread(self, self.sig_node,
# nodes_monitored)
self.sig_node.connect(self._update_output_nodes)
return node_monitor_thread
def _check_nodes_alive(self, signal, nodes_monitored):
"""
Working as a callback of Thread class, this method keeps looping to
watch if the nodes whose names are passed exist and emits signal per
each node.
Notice that what MoveitWidget._check_nodes_alive &
MoveitWidget._check_params_alive do is very similar, but since both of
them are supposed to be passed to Thread class, there might not be
a way to generalize these 2.
@param signal: Signal(bool, str)
@type nodes_monitored: str[]
"""
while self._is_checking_nodes:
rosnode_dynamically_loaded = __import__('rosnode')
#from rosnode import rosnode_ping
for nodename in nodes_monitored:
#TODO: rosnode_ping prints when the node is not found.
# Currently I have no idea how to capture that from here.
try:
#is_node_running = rosnode_ping(nodename, 1)
is_node_running = rosnode_dynamically_loaded.rosnode_ping(
nodename, 1)
except rosnode_dynamically_loaded.ROSNodeIOException as e:
#TODO: Needs to be indicated on GUI
# (eg. PluginContainerWidget)
rospy.logerr(e.message)
is_node_running = False
signal.emit(is_node_running, nodename)
rospy.logdebug('_update_output_nodes')
del rosnode_dynamically_loaded
time.sleep(self._refresh_rate)
def _init_monitor_parameters(self, params_monitored,
_col_names_paramtable=None):
"""
@type params_monitored: str[]
"""
self._params_monitored = params_monitored
self._param_datamodel = QStandardItemModel(0, 2)
self._root_qitem = self._param_datamodel.invisibleRootItem()
self._view_params.setModel(self._param_datamodel)
# Names of header on the QTableView.
if not _col_names_paramtable:
_col_names_paramtable = ['Param name',
'Found on Parameter Server?']
self._param_datamodel.setHorizontalHeaderLabels(_col_names_paramtable)
self.sig_param.connect(self._update_output_parameters)
param_check_thread = Thread(target=self._check_params_alive,
args=(self.sig_param,
self._params_monitored))
return param_check_thread
def _update_output_nodes(self, is_node_running, node_name):
"""
Slot for signals that tell nodes existence.
@type is_node_running: bool
@type node_name: str
"""
#TODO: What this method does is exactly the same with
# monitor_parameters. Unify them.
rospy.logdebug('is_node_running={} par_name={}'.format(is_node_running,
node_name))
node_name = str(node_name)
node_qitem = None
if not node_name in self._node_qitems:
node_qitem = QStandardItem(node_name)
self._node_qitems[node_name] = node_qitem
self._node_datamodel.appendRow(node_qitem)
else: # qsitem with the node name already exists.
node_qitem = self._node_qitems[str(node_name)]
qindex = self._node_datamodel.indexFromItem(node_qitem)
_str_node_running = 'Not running'
if is_node_running:
_str_node_running = 'Running'
qitem_node_status = QStandardItem(_str_node_running)
self._node_datamodel.setItem(qindex.row(), 1, qitem_node_status)
def _check_params_alive(self, signal, params_monitored):
"""
Working as a callback of Thread class, this method keeps looping to
watch if the params whose names are passed exist and emits signal per
each node.
Notice that what MoveitWidget._check_nodes_alive &
MoveitWidget._check_params_alive do is very similar, but since both of
them are supposed to be passed to Thread class, there might not be
a way to generalize these 2.
@type signal: Signal(bool, str)
@param_name signal: emitting a name of the parameter that's found.
@type params_monitored: str[]
"""
while self._is_checking_params:
# self._is_checking_params only turns to false when the plugin
# shuts down.
has_param = False
for param_name in params_monitored:
is_rosmaster_running = RqtRoscommUtil.is_roscore_running()
try:
if is_rosmaster_running:
# Only if rosmaster is running, check if the parameter
# exists or not.
has_param = rospy.has_param(param_name)
except rospy.exceptions.ROSException as e:
self.sig_sysmsg.emit(
'Exception upon rospy.has_param {}'.format(e.message))
signal.emit(has_param, param_name)
rospy.loginfo('has_param {}, check_param_alive: {}'.format(
has_param, param_name))
time.sleep(self._refresh_rate)
def _update_output_parameters(self, has_param, param_name):
"""
Slot
@type has_param: bool
@type param_name: str
"""
rospy.logdebug('has_param={} par_name={}'.format(has_param,
param_name))
param_name = str(param_name)
param_qitem = None
if not param_name in self._param_qitems:
param_qitem = QStandardItem(param_name)
self._param_qitems[param_name] = param_qitem
self._param_datamodel.appendRow(param_qitem)
else: # qsitem with the param name already exists.
param_qitem = self._param_qitems[str(param_name)]
qindex = self._param_datamodel.indexFromItem(param_qitem)
_str_param_found = 'No'
if has_param:
_str_param_found = 'Yes'
qitem_param_status = QStandardItem(_str_param_found)
self._param_datamodel.setItem(qindex.row(), 1, qitem_param_status)
self._view_params.resizeColumnsToContents()
def _update_refreshrate(self, refresh_rate):
"""
Slot
@type refresh_rate: int
"""
self._refresh_rate = refresh_rate
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value(self._SPLITTER_H,
self._splitter.saveState())
def restore_settings(self, plugin_settings, instance_settings):
if instance_settings.contains(self._SPLITTER_H):
self._splitter.restoreState(instance_settings.value(
self._SPLITTER_H))
else:
self._splitter.setSizes([100, 100, 200])
pass
def shutdown(self):
"""
Overridden.
Close threads.
@raise RuntimeError:
"""
try:
#self._node_monitor_thread.join() # No effect
self._is_checking_nodes = False
self._node_monitor_thread = None
#self._param_check_thread.join()
self._is_checking_params = False
self._param_check_thread = None
except RuntimeError as e:
rospy.logerr(e)
raise e
class RqtSrVisualServoing(Plugin):
def __init__(self, context):
super(RqtSrVisualServoing, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('RqtSrVisualServoing')
# # Process standalone plugin command-line arguments
# from argparse import ArgumentParser
# parser = ArgumentParser()
# # Add argument(s) to the parser.
# parser.add_argument("-q", "--quiet", action="store_true",
# dest="quiet",
# help="Put plugin in silent mode")
# args, unknowns = parser.parse_known_args(context.argv())
# if not args.quiet:
# print 'arguments: ', args
# print 'unknowns: ', unknowns
# Create QWidget
self.ui = QWidget()
# Get path to UI file which is in our packages resource directory
rp = rospkg.RosPack()
self.ui_file = os.path.join(rp.get_path(PKG), 'resource', 'gui.ui')
# Extend the widget with all attributes and children from UI file
loadUi(self.ui_file, self.ui)
# Give QObjects reasonable names
self.ui.setObjectName('RqtSrVisualServoingUi')
# Show ui.windowTitle on left-top of each plugin (when it's set in ui).
# 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.ui.setWindowTitle(self.ui.windowTitle() + (' (%d)' % context.serial_number()))
# Wire up the buttons
self.ui.startBtn.clicked.connect( self.start_clicked )
self.ui.stopBtn.clicked.connect( self.stop_clicked )
# Annoyingly setting the icon theme in designer only works in designer,
# we have to set it again here for it to work at run time
self.ui.startBtn.setIcon(QIcon.fromTheme('media-playback-start'))
self.ui.stopBtn.setIcon(QIcon.fromTheme('media-playback-stop'))
# Add widget to the user interface
context.add_widget(self.ui)
# Setup a model for the feedback and link to the view.
self.feedback_model = QStandardItemModel(0,2)
self.feedback_model.setHorizontalHeaderLabels(['Name','Value'])
self.ui.feedbackView.setModel(self.feedback_model)
self.ui.connect( self.ui, SIGNAL('feedback(QString)'), self.update_feedback )
# ROS setup
self.last_feedback = None
self.client = actionlib.SimpleActionClient('visual_servo', VisualServoingAction)
msg = ""
if self.client.wait_for_server(rospy.Duration(2.0)):
msg = "Found action server, servoing appears to be running"
rospy.loginfo(msg)
else:
msg = "Can't find action server, servoing not running"
rospy.logerr(msg)
self.ui.statusValue.setText(msg)
def shutdown_plugin(self):
# TODO unregister all publishers here
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 trigger_configuration(self):
# Comment in to signal that the plugin has a way to configure it
# Usually used to open a configuration dialog
def start_clicked(self):
goal = VisualServoingActionGoal()
self.client.send_goal(goal, feedback_cb = self._feedback_cb)
self.ui.statusValue.setText("Starting")
def stop_clicked(self):
self.client.cancel_all_goals()
self.ui.statusValue.setText("Stopped")
self.feedback_model.clear()
def _feedback_cb(self, feedback):
# We can't update the UI in this thread so stash the data and emit a
# signal that can be traped by the main thread and update the ui.
self.last_feedback = feedback
self.ui.emit( SIGNAL('feedback(QString)'), "" )
def update_feedback(self, data):
"""Listen for feedback signals and update the interface."""
fb = self.last_feedback
self.ui.statusValue.setText(str(self.client.get_goal_status_text()))
# Update the feedback model, which triggers the view to update
m = self.feedback_model
m.setHorizontalHeaderLabels(['Name','Value'])
m.setItem(0,0,QStandardItem('distance'))
m.setItem(0,1,QStandardItem(str(fb.distance)))
m.setItem(1,0,QStandardItem('object_pose.position.x'))
m.setItem(1,1,QStandardItem(str(fb.object_pose.position.x)))
m.setItem(2,0,QStandardItem('object_pose.position.y'))
m.setItem(2,1,QStandardItem(str(fb.object_pose.position.y)))
m.setItem(3,0,QStandardItem('object_pose.position.z'))
m.setItem(3,1,QStandardItem(str(fb.object_pose.position.z)))
m.setItem(4,0,QStandardItem('object_pose.orientation.x'))
m.setItem(4,1,QStandardItem(str(fb.object_pose.orientation.x)))
m.setItem(5,0,QStandardItem('object_pose.orientation.y'))
m.setItem(5,1,QStandardItem(str(fb.object_pose.orientation.y)))
m.setItem(6,0,QStandardItem('object_pose.orientation.z'))
m.setItem(6,1,QStandardItem(str(fb.object_pose.orientation.z)))
m.setItem(7,0,QStandardItem('object_pose.orientation.w'))
m.setItem(7,1,QStandardItem(str(fb.object_pose.orientation.w)))
m.setItem(8,0,QStandardItem('grasp_pose.position.x'))
m.setItem(8,1,QStandardItem(str(fb.grasp_pose.position.x)))
m.setItem(9,0,QStandardItem('grasp_pose.position.y'))
m.setItem(9,1,QStandardItem(str(fb.grasp_pose.position.y)))
m.setItem(10,0,QStandardItem('grasp_pose.position.z'))
m.setItem(10,1,QStandardItem(str(fb.grasp_pose.position.z)))
m.setItem(11,0,QStandardItem('grasp_pose.orientation.x'))
m.setItem(11,1,QStandardItem(str(fb.grasp_pose.orientation.x)))
m.setItem(12,0,QStandardItem('grasp_pose.orientation.y'))
m.setItem(12,1,QStandardItem(str(fb.grasp_pose.orientation.y)))
m.setItem(13,0,QStandardItem('grasp_pose.orientation.z'))
m.setItem(13,1,QStandardItem(str(fb.grasp_pose.orientation.z)))
m.setItem(14,0,QStandardItem('grasp_pose.orientation.w'))
m.setItem(14,1,QStandardItem(str(fb.grasp_pose.orientation.w)))
self.ui.feedbackView.resizeColumnsToContents()
class MoveitWidget(QWidget):
"""
This Widget provides an overview about the presence of different parts of a running moveIt instance.
"""
# To be connected to PluginContainerWidget
sig_sysmsg = Signal(str)
sig_param = Signal(bool, str) # param name emitted
sig_node = Signal(bool, str) # node name emitted
sig_topic = Signal(list) # topic name emitted
_SPLITTER_H = 'splitter_horizontal'
def __init__(self, parent, plugin_context):
"""
@type parent: MoveitMain
"""
self._ros_master = xmlrpclib.ServerProxy(os.environ['ROS_MASTER_URI'])
self._stop_event = threading.Event()
self._nodes_monitored = ['/move_group']
self._selected_topics = [('/pointcloud', 'sensor_msgs/PointCloud'),
('/pointcloud2', 'sensor_msgs/PointCloud2'),
('/image', 'sensor_msgs/Image'),
('/camera_info', 'sensor_msgs/CameraInfo')]
self._params_monitored = [
'/robot_description', '/robot_description_semantic'
]
super(MoveitWidget, self).__init__()
self._parent = parent
self._plugin_context = plugin_context
self._refresh_rate = 5 # With default value
self._rospack = rospkg.RosPack()
ui_file = os.path.join(self._rospack.get_path('rqt_moveit'),
'resource', 'moveit_top.ui')
loadUi(ui_file, self, {'TopicWidget': TopicWidget})
# Custom widget classes don't show in QSplitter when they instantiated
# in .ui file and not explicitly added to QSplitter like this. Thus
# this is a workaround.
self._splitter.addWidget(self._widget_topic)
self._spinbox_refreshrate.valueChanged.connect(
self._update_refreshrate)
# Show default ref rate on QSpinbox
self._spinbox_refreshrate.setValue(self._refresh_rate)
# Monitor node
self._node_qitems = {}
self._node_monitor_thread = self._init_monitor_nodes()
self._node_monitor_thread.start()
# topic to show
self._registered_topics = None
self._topic_monitor_thread = self._init_monitor_topics()
self._topic_monitor_thread.start()
# Init monitoring parameters.
self._param_qitems = {}
_col_names_paramtable = ['Param name', 'Found on Parameter Server?']
self._param_check_thread = self._init_monitor_parameters(
_col_names_paramtable)
self._param_check_thread.start()
def _init_monitor_nodes(self):
"""
@rtype: Thread
"""
self._node_datamodel = QStandardItemModel(0, 2)
self._root_qitem = self._node_datamodel.invisibleRootItem()
self._view_nodes.setModel(self._node_datamodel)
node_monitor_thread = threading.Thread(target=self._check_nodes_alive,
args=(self.sig_node,
self._nodes_monitored,
self._stop_event))
self.sig_node.connect(self._update_output_nodes)
return node_monitor_thread
def _check_nodes_alive(self, signal, nodes_monitored, stop_event):
"""
Working as a callback of Thread class, this method keeps looping to
watch if the nodes whose names are passed exist and emits signal per
each node.
Notice that what MoveitWidget._check_nodes_alive &
MoveitWidget._check_params_alive do is very similar, but since both of
them are supposed to be passed to Thread class, there might not be
a way to generalize these 2.
@param signal: Signal(bool, str)
@type nodes_monitored: str[]
@type stop_event: Event()
"""
rosnode_dynamically_loaded = __import__('rosnode')
while True:
for nodename in nodes_monitored:
try:
registered_nodes = rosnode_dynamically_loaded.get_node_names(
)
is_node_running = nodename in registered_nodes
except rosnode_dynamically_loaded.ROSNodeIOException as e:
# TODO: Needs to be indicated on GUI
# (eg. PluginContainerWidget)
rospy.logerr(e.message)
is_node_running = False
signal.emit(is_node_running, nodename)
rospy.logdebug('_update_output_nodes')
if stop_event.wait(self._refresh_rate):
del rosnode_dynamically_loaded
return
def _init_monitor_topics(self):
"""
@rtype: Thread
"""
topic_monitor_thread = threading.Thread(
target=self._check_topics_alive,
args=(self.sig_topic, self._selected_topics, self._stop_event))
self.sig_topic.connect(self._update_output_topics)
return topic_monitor_thread
def _check_topics_alive(self, signal, topics_monitored, stop_event):
"""
Working as a callback of Thread class, this method keeps looping to
watch if the topics whose names are passed exist and emits signal per
each node.
@param signal: Signal()
@type topics_monitored: str[]
@type stop_event: Event()
"""
while True:
code, msg, val = self._ros_master.getPublishedTopics(
'/rqt_moveit_update_script', "")
if code == 1:
published_topics = dict(val)
else:
rospy.logerr("Communication with rosmaster failed")
registered_topics = []
for topic in topics_monitored:
if topic[0] in published_topics and topic[
1] == published_topics.get(topic[0]):
registered_topics.append(
(topic[0], published_topics.get(topic[0])))
signal.emit(list(registered_topics))
rospy.logdebug('_update_output_topics')
if stop_event.wait(self._refresh_rate):
return
def _init_monitor_parameters(self, _col_names_paramtable=None):
"""
@rtype: Thread
"""
self._param_datamodel = QStandardItemModel(0, 2)
self._root_qitem = self._param_datamodel.invisibleRootItem()
self._view_params.setModel(self._param_datamodel)
# Names of header on the QTableView.
if not _col_names_paramtable:
_col_names_paramtable = [
'Param name', 'Found on Parameter Server?'
]
self._param_datamodel.setHorizontalHeaderLabels(_col_names_paramtable)
self.sig_param.connect(self._update_output_parameters)
param_check_thread = threading.Thread(target=self._check_params_alive,
args=(self.sig_param,
self._params_monitored,
self._stop_event))
return param_check_thread
def _update_output_nodes(self, is_node_running, node_name):
"""
Slot for signals that tell nodes existence.
@type is_node_running: bool
@type node_name: str
"""
# TODO: What this method does is exactly the same with
# monitor_parameters. Unify them.
rospy.logdebug('is_node_running={} par_name={}'.format(
is_node_running, node_name))
node_name = str(node_name)
node_qitem = None
if node_name not in self._node_qitems:
node_qitem = QStandardItem(node_name)
self._node_qitems[node_name] = node_qitem
self._node_datamodel.appendRow(node_qitem)
else: # qsitem with the node name already exists.
node_qitem = self._node_qitems[str(node_name)]
qindex = self._node_datamodel.indexFromItem(node_qitem)
_str_node_running = 'Not running'
if is_node_running:
_str_node_running = 'Running'
qitem_node_status = QStandardItem(_str_node_running)
self._node_datamodel.setItem(qindex.row(), 1, qitem_node_status)
def _update_output_topics(self, registered_topics):
"""
Slot for signals that tell topic's existence.
@type registered_topics: list
"""
# This branch will cause that once a selected topic was found the topic view will
# never be empty again.
if len(registered_topics) > 0:
if self._registered_topics is None:
self._widget_topic.set_selected_topics(registered_topics)
self._widget_topic.set_topic_specifier(
TopicWidget.SELECT_BY_NAME)
self._widget_topic.start()
elif self._registered_topics is not None and set(
self._registered_topics) != set(registered_topics):
self._widget_topic.set_selected_topics(registered_topics)
self._registered_topics = registered_topics
def _check_params_alive(self, signal, params_monitored, stop_event):
"""
Working as a callback of Thread class, this method keeps looping to
watch if the params whose names are passed exist and emits signal per
each node.
Notice that what MoveitWidget._check_nodes_alive &
MoveitWidget._check_params_alive do is very similar, but since both of
them are supposed to be passed to Thread class, there might not be
a way to generalize these 2.
@type signal: Signal(bool, str)
@param_name signal: emitting a name of the parameter that's found.
@type params_monitored: str[]
@type stop_event: Event()
"""
while True:
has_param = False
for param_name in params_monitored:
is_rosmaster_running = RqtRoscommUtil.is_roscore_running()
try:
if is_rosmaster_running:
# Only if rosmaster is running, check if the parameter
# exists or not.
has_param = rospy.has_param(param_name)
except rospy.exceptions.ROSException as e:
rospy.logerr('Exception upon rospy.has_param {}'.format(
e.message))
self.sig_sysmsg.emit(
'Exception upon rospy.has_param {}'.format(e.message))
signal.emit(has_param, param_name)
rospy.logdebug('has_param {}, check_param_alive: {}'.format(
has_param, param_name))
if stop_event.wait(self._refresh_rate):
return
def _update_output_parameters(self, has_param, param_name):
"""
Slot
@type has_param: bool
@type param_name: str
"""
rospy.logdebug('has_param={} par_name={}'.format(
has_param, param_name))
param_name = str(param_name)
param_qitem = None
if param_name not in self._param_qitems:
param_qitem = QStandardItem(param_name)
self._param_qitems[param_name] = param_qitem
self._param_datamodel.appendRow(param_qitem)
else: # qsitem with the param name already exists.
param_qitem = self._param_qitems[str(param_name)]
qindex = self._param_datamodel.indexFromItem(param_qitem)
_str_param_found = 'No'
if has_param:
_str_param_found = 'Yes'
qitem_param_status = QStandardItem(_str_param_found)
self._param_datamodel.setItem(qindex.row(), 1, qitem_param_status)
self._view_params.resizeColumnsToContents()
def _update_refreshrate(self, refresh_rate):
"""
Slot
@type refresh_rate: int
"""
self._refresh_rate = refresh_rate
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value(self._SPLITTER_H,
self._splitter.saveState())
def restore_settings(self, plugin_settings, instance_settings):
if instance_settings.contains(self._SPLITTER_H):
self._splitter.restoreState(
instance_settings.value(self._SPLITTER_H))
else:
self._splitter.setSizes([100, 100, 200])
pass
def shutdown(self):
"""
Overridden.
Close threads.
@raise RuntimeError:
"""
try:
self._stop_event.set()
self._node_monitor_thread.join()
self._param_check_thread.join()
self._topic_monitor_thread.join()
self._node_monitor_thread = None
self._param_check_thread = None
self._topic_monitor_thread = None
except RuntimeError as e:
rospy.logerr(e)
raise e
class ChatbotGUI(Plugin):
def __init__(self, context):
super(ChatbotGUI, self).__init__(context)
self.setObjectName('ChatbotGUI')
from argparse import ArgumentParser
parser = ArgumentParser()
parser.add_argument("-q",
"--quiet",
action="store_true",
dest="quiet",
help="Put plugin in silent mode")
args, unknowns = parser.parse_known_args(context.argv())
if not args.quiet:
print 'arguments: ', args
print 'unknowns: ', unknowns
self._widget = QWidget()
ui_file = os.path.join(rospkg.RosPack().get_path('chatbot_gui'),
'resource', 'ChatbotGUI.ui')
loadUi(ui_file, self._widget, {'NavViewWidget': NavViewWidget})
self._widget.setObjectName('ChatbotGUIUi')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
context.add_widget(self._widget)
self._widget.pickDone.clicked[bool].connect(self._handle_pick_clicked)
self._widget.placeDone.clicked[bool].connect(
self._handle_place_clicked)
self._widget.textSay.keyPressEvent = self._handle_custom_keypress
#ros stuff
self.pubPick = rospy.Publisher('picker', String, queue_size=10)
self.pubPlace = rospy.Publisher('placer', String, queue_size=10)
self.pubRecog = rospy.Publisher('recognition/raw_result',
String,
queue_size=10)
self.subRecog = rospy.Subscriber("recognition/raw_result", String,
self._recog_callback)
self.subLog = rospy.Subscriber("chatbot_gui/log", String,
self._log_callback)
self.subStatus = rospy.Subscriber("chatbot_gui/status", String,
self._status_callback)
self.model = QStandardItemModel()
self.model.setColumnCount(2)
headerNames = []
headerNames.append("Timestamp")
headerNames.append("Message")
self.model.setHorizontalHeaderLabels(headerNames)
self._widget.tableLog.setModel(self.model)
self._widget.tableLog.horizontalHeader().setStretchLastSection(True)
self._widget.tableLog.setColumnWidth(0, 170)
self._message_limit = 20000
self._message_count = 0
def _add_log(self, data):
timestamp = QStandardItem(
QDateTime.currentDateTime().toString('d.M.yyyy hh:mm:ss.zzz'))
timestamp.setTextAlignment(Qt.AlignRight)
timestamp.setEditable(False)
message = QStandardItem(data)
message.setEditable(False)
row = []
row.append(timestamp)
row.append(message)
self._message_count = self._message_count + 1
self.model.insertRow(0, row)
if self._message_count > self._message_limit:
self.model.removeRow(self.model.rowCount() - 1)
self._message_count = self._message_count - 1
def _handle_pick_clicked(self):
self.pubPick.publish("[pick done]")
def _handle_place_clicked(self):
self.pubPlace.publish("[place done]")
def _handle_custom_keypress(self, event):
if event.key() == Qt.Key_Return or event.key() == Qt.Key_Enter:
self.pubRecog.publish(self._widget.textSay.toPlainText())
self._widget.textSay.setPlainText("")
else:
QPlainTextEdit.keyPressEvent(self._widget.textSay, event)
def _recog_callback(self, data):
self._add_log("heard: '" + data.data + "'")
def _log_callback(self, data):
self._add_log(data.data)
def _status_callback(self, data):
self._widget.labelStatus.setText(data.data)
def shutdown_plugin(self):
print("shutdown_plugin: clearing ros publishers.")
self.subRecog.unregister()
self.subStatus.unregister()
self.subLog.unregister()
self.pubPick.unregister()
self.pubPlace.unregister()
self.pubRecog.unregister()
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
