def __init__(self, parent=None):
super(LaunchWidget, self).__init__(parent)
self._topic_dict = {}
self._update_thread = WorkerThread(self._update_thread_run,
self._update_finished)
self._rospack = rospkg.RosPack()
ui_file = os.path.join(self._rospack.get_path('roslaunch_monitor'),
'resource', 'rqt_monitor_plugin.ui')
loadUi(
ui_file, self, {
'ExtendedComboBox': ExtendedComboBox,
'LaunchTreeWidget': LaunchTreeWidget
})
self.refresh_button.setIcon(QIcon.fromTheme('view-refresh'))
self.refresh_button.clicked.connect(self.refresh_combo_boxes)
self.add_launch_button.setIcon(QIcon.fromTheme('add'))
self.remove_launch_button.setIcon(QIcon.fromTheme('remove'))
self.clear_button.setIcon(QIcon.fromTheme('edit-clear'))
self.refresh_combo_boxes()
self.launch_tree_widget.model().item_value_changed.connect(
self.change_launch)
self.launch_tree_widget.remove_publisher.connect(self.remove_launch)
#self.publisher_tree_widget.publish_once.connect(self.publish_once)
self.remove_launch_button.clicked.connect(
self.launch_tree_widget.remove_selected_publishers)
self.clear_button.clicked.connect(self.clean_up_launches)
def __init__(self, node, parent=None):
super(PublisherWidget, self).__init__(parent)
self._node = node
self._topic_dict = {}
self._update_thread = WorkerThread(self._update_thread_run,
self._update_finished)
package_path = get_package_path('rqt_publisher')
ui_file = os.path.join(package_path, 'share', 'rqt_publisher',
'resource', 'Publisher.ui')
loadUi(
ui_file, self, {
'ExtendedComboBox': ExtendedComboBox,
'PublisherTreeWidget': PublisherTreeWidget
})
self.refresh_button.setIcon(QIcon.fromTheme('view-refresh'))
self.refresh_button.clicked.connect(self.refresh_combo_boxes)
self.add_publisher_button.setIcon(QIcon.fromTheme('list-add'))
self.remove_publisher_button.setIcon(QIcon.fromTheme('list-remove'))
self.clear_button.setIcon(QIcon.fromTheme('edit-clear'))
self.refresh_combo_boxes()
self.publisher_tree_widget.model().item_value_changed.connect(
self.change_publisher)
self.publisher_tree_widget.remove_publisher.connect(
self.remove_publisher)
self.publisher_tree_widget.publish_once.connect(self.publish_once)
self.remove_publisher_button.clicked.connect(
self.publisher_tree_widget.remove_selected_publishers)
self.clear_button.clicked.connect(self.clean_up_publishers)
def on_start_button_clicked(self):
self._ui.start_button.setEnabled(False)
self._ui.stop_button.setEnabled(True)
if not self._scan_to_angle:
self._scan_to_angle = ScanToAngle('/scan',self._laser_scan_angle_topic_name)
if not self._motion:
self._motion = DriftEstimation(self._laser_scan_angle_topic_name, self._gyro_scan_angle_topic_name, self._error_scan_angle_topic_name, self._cmd_vel_topic_name,self._gyro_topic_name)
self._motion.init(self._ui.angular_speed_spinbox.value())
rospy.sleep(0.5)
self._plot_widget.data_plot.reset()
self._plot_widget._start_time = rospy.get_time()
self._plot_widget.enable_timer(True)
try:
self._plot_widget.remove_topic(self._error_scan_angle_topic_name+'/scan_angle')
self._plot_widget_live.remove_topic(self._laser_scan_angle_topic_name+'/scan_angle')
self._plot_widget_live.remove_topic(self._gyro_scan_angle_topic_name+'/scan_angle')
self._plot_widget_live.remove_topic(self._cmd_vel_topic_name+'/angular/z')
except KeyError:
pass
self._plot_widget_live.add_topic(self._cmd_vel_topic_name+'/angular/z')
self._plot_widget_live.add_topic(self._laser_scan_angle_topic_name+'/scan_angle')
self._plot_widget_live.add_topic(self._gyro_scan_angle_topic_name+'/scan_angle')
self._plot_widget.add_topic(self._error_scan_angle_topic_name+'/scan_angle')
self._motion_thread = WorkerThread(self._motion.execute, None)
self._motion_thread.start()
def on_start_button_clicked(self):
self._ui.start_button.setEnabled(False)
self._ui.stop_button.setEnabled(True)
self._state = CliffSensorFrame.STATE_FORWARD
self._motion_thread = WorkerThread(self._motion.execute,
self._run_finished)
self._motion_thread.start()
def on_start_button_clicked(self):
self.start_button.setEnabled(False)
self.stop_button.setEnabled(True)
self._motion_thread = WorkerThread(self._motion.execute_speed,
self._run_finished)
self._motion_thread.start()
#rospy.loginfo("current:"+self._config.get_cmd_topic())
self._motion.change_topic(self._config.get_cmd_topic())
self._config.run(False, False, False, True)
def on_start_button_clicked(self):
self._ui.start_button.setEnabled(False)
self._ui.stop_button.setEnabled(True)
self._motion = Square('/mobile_base/commands/velocity', '/odom',
self._gyro_topic_name)
self._motion.init(self._ui.speed_spinbox.value(),
self._ui.distance_spinbox.value())
self._motion_thread = WorkerThread(self._motion.execute,
self._run_finished)
self._motion_thread.start()
def create_editor(self):
editor = FrameEditor()
editor.observers.append(self)
self.signal_update.connect(self.update_all)
self._update_thread = WorkerThread(self._update_thread_run, self._update_finished)
self.old_frame_list = []
self.old_selected = ""
return editor
def __init__(self, parent=None):
super(MsgEditorWidget, self).__init__(parent)
self._topic_dict = {}
self._update_thread = WorkerThread(self._update_thread_run, self._update_finished)
self._rospack = rospkg.RosPack()
ui_file = os.path.join(self._rospack.get_path('rqt_annotation_data'), 'resource', 'MsgEditor.ui')
loadUi(ui_file, self, {'ExtendedComboBox': ExtendedComboBox, 'MessageTreeWidget': MessageTreeWidget})
self.refresh_combo_boxes()
self.message_tree_widget.model().item_value_changed.connect(self.change_message)
self.clear_button.clicked.connect(self.clean)
def _run_finished(self):
if self._state == CliffSensorFrame.STATE_STOPPED:
return
elif self._state == CliffSensorFrame.STATE_FORWARD:
self._state = CliffSensorFrame.STATE_BACKWARD
self._motion.init(-self._motion.speed, 0.2)
self._motion_thread = WorkerThread(self._motion.execute, self._run_finished)
self._motion_thread.start()
else:
self._state = CliffSensorFrame.STATE_FORWARD
self._motion.init(-self._motion.speed, self._distance)
self._motion_thread = WorkerThread(self._motion.execute, self._run_finished)
self._motion_thread.start()
def _run_finished(self):
if self._state == CliffSensorFrame.STATE_STOPPED:
return
elif self._state == CliffSensorFrame.STATE_FORWARD:
self._state = CliffSensorFrame.STATE_BACKWARD
self._motion.init(-self._motion.speed, 0.2)
self._motion_thread = WorkerThread(self._motion.execute,
self._run_finished)
self._motion_thread.start()
else:
self._state = CliffSensorFrame.STATE_FORWARD
self._motion.init(-self._motion.speed, self._distance)
self._motion_thread = WorkerThread(self._motion.execute,
self._run_finished)
self._motion_thread.start()
def on_start_button_clicked(self):
self._ui.start_button.setEnabled(False)
self._ui.stop_button.setEnabled(True)
if not self._scan_to_angle:
self._scan_to_angle = ScanToAngle("/scan", self._laser_scan_angle_topic_name)
if not self._motion:
self._motion = DriftEstimation(
self._laser_scan_angle_topic_name,
self._gyro_scan_angle_topic_name,
self._error_scan_angle_topic_name,
self._cmd_vel_topic_name,
self._gyro_topic_name,
)
self._motion.init(self._ui.angular_speed_spinbox.value())
rospy.sleep(0.5)
self._plot_widget.data_plot.reset()
self._plot_widget._start_time = rospy.get_time()
self._plot_widget.enable_timer(True)
try:
self._plot_widget.remove_topic(self._error_scan_angle_topic_name + "/scan_angle")
self._plot_widget_live.remove_topic(self._laser_scan_angle_topic_name + "/scan_angle")
self._plot_widget_live.remove_topic(self._gyro_scan_angle_topic_name + "/scan_angle")
self._plot_widget_live.remove_topic(self._cmd_vel_topic_name + "/angular/z")
except KeyError:
pass
self._plot_widget_live.add_topic(self._cmd_vel_topic_name + "/angular/z")
self._plot_widget_live.add_topic(self._laser_scan_angle_topic_name + "/scan_angle")
self._plot_widget_live.add_topic(self._gyro_scan_angle_topic_name + "/scan_angle")
self._plot_widget.add_topic(self._error_scan_angle_topic_name + "/scan_angle")
self._motion_thread = WorkerThread(self._motion.execute, None)
self._motion_thread.start()
def on_start_button_clicked(self):
self._ui.start_button.setEnabled(False)
self._ui.stop_button.setEnabled(True)
self._motion = Square('/mobile_base/commands/velocity', '/odom', self._gyro_topic_name)
self._motion.init(self._ui.speed_spinbox.value(), self._ui.distance_spinbox.value())
self._motion_thread = WorkerThread(self._motion.execute, self._run_finished)
self._motion_thread.start()
def on_start_button_clicked(self):
self._ui.start_button.setEnabled(False)
self._ui.stop_button.setEnabled(True)
if not self._motion:
self._motion = Rotate(self._cmd_vel_topic_name)
self._motion.init(self._ui.angular_speed_spinbox.value())
if not self.robot_state_subscriber:
self.robot_state_subscriber = rospy.Subscriber("/mobile_base/events/robot_state", RobotStateEvent, self.robot_state_callback)
rospy.sleep(0.5)
self._plot_widget._start_time = rospy.get_time()
self._plot_widget.enable_timer(True)
try:
self._plot_widget.remove_topic(self._battery_topic_name)
except KeyError:
pass
self._plot_widget.add_topic(self._battery_topic_name)
self._motion_thread = WorkerThread(self._motion.execute, self._run_finished)
self._motion_thread.start()
def on_start_button_clicked(self):
self._ui.start_button.setEnabled(False)
self._ui.stop_button.setEnabled(True)
if not self._motion:
self._motion = Rotate(self._cmd_vel_topic_name)
self._motion.init(self._ui.angular_speed_spinbox.value())
if not self.robot_state_subscriber:
self.robot_state_subscriber = rospy.Subscriber("/mobile_base/events/robot_state", RobotStateEvent, self.robot_state_callback)
rospy.sleep(0.5)
self._plot_widget._start_time = rospy.get_time()
self._plot_widget.enable_timer(True)
try:
self._plot_widget.remove_topic(self._battery_topic_name)
except KeyError:
pass
self._plot_widget.add_topic(self._battery_topic_name)
self._motion_thread = WorkerThread(self._motion.execute, self._run_finished)
self._motion_thread.start()
def __init__(self, parent=None):
super(PublisherWidget, self).__init__(parent)
self._topic_dict = {}
self._update_thread = WorkerThread(self._update_thread_run, self._update_finished)
self._rospack = rospkg.RosPack()
ui_file = os.path.join(self._rospack.get_path('rqt_publisher'), 'resource', 'Publisher.ui')
loadUi(ui_file, self, {'ExtendedComboBox': ExtendedComboBox, 'PublisherTreeWidget': PublisherTreeWidget})
self.refresh_button.setIcon(QIcon.fromTheme('view-refresh'))
self.refresh_button.clicked.connect(self.refresh_combo_boxes)
self.add_publisher_button.setIcon(QIcon.fromTheme('list-add'))
self.remove_publisher_button.setIcon(QIcon.fromTheme('list-remove'))
self.clear_button.setIcon(QIcon.fromTheme('edit-clear'))
self.refresh_combo_boxes()
self.publisher_tree_widget.model().item_value_changed.connect(self.change_publisher)
self.publisher_tree_widget.remove_publisher.connect(self.remove_publisher)
self.publisher_tree_widget.publish_once.connect(self.publish_once)
self.remove_publisher_button.clicked.connect(self.publisher_tree_widget.remove_selected_publishers)
self.clear_button.clicked.connect(self.clean_up_publishers)
class BatteryProfileFrame(QFrame):
def __init__(self, parent=None):
super(BatteryProfileFrame, self).__init__(parent)
self._cmd_vel_topic_name = '/mobile_base/commands/velocity'
self._battery_topic_name = "/mobile_base/sensors/core/battery"
self._ui = Ui_battery_profile_frame()
self._motion = None
self.robot_state_subscriber = None
self._motion_thread = None
def setupUi(self, cmd_vel_topic_name):
self._ui.setupUi(self)
self._cmd_vel_topic_name = cmd_vel_topic_name
self._plot_layout = QVBoxLayout(self._ui.battery_profile_group_box)
self._plot_widget = PlotWidget()
self._plot_widget.setWindowTitle("Battery Profile")
self._plot_widget.topic_edit.setText(self._battery_topic_name)
self._plot_layout.addWidget(self._plot_widget)
self._data_plot = DataPlot(self._plot_widget)
self._data_plot.set_autoscale(y=False)
self._data_plot.set_ylim([0, 180])
self._plot_widget.switch_data_plot_widget(self._data_plot)
self._ui.start_button.setEnabled(True)
self._ui.stop_button.setEnabled(False)
def shutdown(self):
'''
Used to terminate the plugin
'''
rospy.loginfo("Kobuki TestSuite: battery test shutdown")
self._stop()
##########################################################################
# Widget Management
##########################################################################
def hibernate(self):
'''
This gets called when the frame goes out of focus (tab switch).
Disable everything to avoid running N tabs in parallel when in
reality we are only running one.
'''
self._stop()
def restore(self):
'''
Restore the frame after a hibernate.
'''
pass
##########################################################################
# Motion Callbacks
##########################################################################
def _run_finished(self):
self._ui.start_button.setEnabled(True)
self._ui.stop_button.setEnabled(False)
##########################################################################
# Qt Callbacks
##########################################################################
@Slot()
def on_start_button_clicked(self):
self._ui.start_button.setEnabled(False)
self._ui.stop_button.setEnabled(True)
if not self._motion:
self._motion = Rotate(self._cmd_vel_topic_name)
self._motion.init(self._ui.angular_speed_spinbox.value())
if not self.robot_state_subscriber:
self.robot_state_subscriber = rospy.Subscriber("/mobile_base/events/robot_state", RobotStateEvent, self.robot_state_callback)
rospy.sleep(0.5)
self._plot_widget._start_time = rospy.get_time()
self._plot_widget.enable_timer(True)
try:
self._plot_widget.remove_topic(self._battery_topic_name)
except KeyError:
pass
self._plot_widget.add_topic(self._battery_topic_name)
self._motion_thread = WorkerThread(self._motion.execute, self._run_finished)
self._motion_thread.start()
@Slot()
def on_stop_button_clicked(self):
'''
Hardcore stoppage - straight to zero.
'''
self._stop()
def _stop(self):
self._plot_widget.enable_timer(False) # pause plot rendering
if self._motion_thread:
self._motion.stop()
self._motion_thread.wait()
self._motion_thread = None
if self._motion:
self._motion.shutdown()
self._motion = None
if self.robot_state_subscriber:
self.robot_state_subscriber.unregister()
self.robot_state_subscriber = None
self._ui.start_button.setEnabled(True)
self._ui.stop_button.setEnabled(False)
@pyqtSlot(float)
def on_angular_speed_spinbox_valueChanged(self, value):
if self._motion:
self._motion.init(self._ui.angular_speed_spinbox.value())
##########################################################################
# External Slot Callbacks
##########################################################################
@Slot(str)
def on_cmd_vel_topic_combo_box_currentIndexChanged(self, topic_name):
'''
To be connected to the configuration dock combo box (via the main testsuite frame)
'''
self._cmd_vel_topic_name = topic_name
print("DudetteBattery %s" % topic_name)
##########################################################################
# Ros Callbacks
##########################################################################
def robot_state_callback(self, data):
if data.state == RobotStateEvent.OFFLINE:
self.stop()
class PayloadFrame(QFrame):
def __init__(self, parent=None):
super(PayloadFrame, self).__init__(parent)
self._gyro_topic_name = '/mobile_base/sensors/imu_data'
self._ui = Ui_payload_frame()
self._motion = None
self._motion_thread = None
def setupUi(self):
self._ui.setupUi(self)
self._ui.start_button.setEnabled(True)
self._ui.stop_button.setEnabled(False)
def shutdown(self):
'''
Used to terminate the plugin
'''
rospy.loginfo("Kobuki TestSuite: payload frame shutdown")
self._stop()
##########################################################################
# Widget Management
##########################################################################
def hibernate(self):
'''
This gets called when the frame goes out of focus (tab switch).
Disable everything to avoid running N tabs in parallel when in
reality we are only running one.
'''
self._stop()
def restore(self):
'''
Restore the frame after a hibernate.
'''
pass
##########################################################################
# Motion Callbacks
##########################################################################
def _run_finished(self):
self._motion_thread = None
self._motion = None
self._ui.start_button.setEnabled(True)
self._ui.stop_button.setEnabled(False)
##########################################################################
# Qt Callbacks
##########################################################################
@Slot()
def on_start_button_clicked(self):
self._ui.start_button.setEnabled(False)
self._ui.stop_button.setEnabled(True)
self._motion = Square('/mobile_base/commands/velocity', '/odom', self._gyro_topic_name)
self._motion.init(self._ui.speed_spinbox.value(), self._ui.distance_spinbox.value())
self._motion_thread = WorkerThread(self._motion.execute, self._run_finished)
self._motion_thread.start()
@Slot()
def on_stop_button_clicked(self):
self._stop()
def _stop(self):
if self._motion:
self._motion.stop()
if self._motion_thread:
self._motion_thread.wait()
self._motion_thread = None
if self._motion:
self._motion = None
self._ui.start_button.setEnabled(True)
self._ui.stop_button.setEnabled(False)
@pyqtSlot(float)
def on_speed_spinbox_valueChanged(self, value):
if self._motion:
self._motion.init(self._ui.speed_spinbox.value(), self._ui.distance_spinbox.value())
@pyqtSlot(float)
def on_distance_spinbox_valueChanged(self, value):
if self._motion:
self._motion.init(self._ui.speed_spinbox.value(), self._ui.distance_spinbox.value())
class LaunchWidget(QWidget):
add_launch = Signal(str, str, str, bool)
change_launch = Signal(int, str, str, str, object)
#publish_once = Signal(int)
remove_launch = Signal(int)
clean_up_launches = Signal()
def __init__(self, parent=None):
super(LaunchWidget, self).__init__(parent)
self._topic_dict = {}
self._update_thread = WorkerThread(self._update_thread_run,
self._update_finished)
self._rospack = rospkg.RosPack()
ui_file = os.path.join(self._rospack.get_path('roslaunch_monitor'),
'resource', 'rqt_monitor_plugin.ui')
loadUi(
ui_file, self, {
'ExtendedComboBox': ExtendedComboBox,
'LaunchTreeWidget': LaunchTreeWidget
})
self.refresh_button.setIcon(QIcon.fromTheme('view-refresh'))
self.refresh_button.clicked.connect(self.refresh_combo_boxes)
self.add_launch_button.setIcon(QIcon.fromTheme('add'))
self.remove_launch_button.setIcon(QIcon.fromTheme('remove'))
self.clear_button.setIcon(QIcon.fromTheme('edit-clear'))
self.refresh_combo_boxes()
self.launch_tree_widget.model().item_value_changed.connect(
self.change_launch)
self.launch_tree_widget.remove_publisher.connect(self.remove_launch)
#self.publisher_tree_widget.publish_once.connect(self.publish_once)
self.remove_launch_button.clicked.connect(
self.launch_tree_widget.remove_selected_publishers)
self.clear_button.clicked.connect(self.clean_up_launches)
def shutdown_plugin(self):
self._update_thread.kill()
@Slot()
def refresh_combo_boxes(self):
self._update_thread.kill()
self.file_combo_box.setEnabled(False)
self.package_combo_box.setEnabled(False)
self.file_combo_box.setEditText('updating...')
self.package_combo_box.setEditText('updating...')
self._update_thread.start()
# this runs in a non-gui thread, so don't access widgets here directly
def _update_thread_run(self):
# update type_combo_box
message_type_names = []
try:
# this only works on fuerte and up
packages = sorted([
pkg_tuple[0] for pkg_tuple in rosmsg.iterate_packages(
self._rospack, rosmsg.MODE_MSG)
])
except:
# this works up to electric
packages = sorted(rosmsg.list_packages())
for package in packages:
for base_type_str in rosmsg.list_msgs(package,
rospack=self._rospack):
message_class = roslib.message.get_message_class(base_type_str)
if message_class is not None:
message_type_names.append(base_type_str)
# TODO: get all ROS packages and launch files here instead
#self.type_combo_box.setItems.emit(sorted(message_type_names))
packages = sorted([
pkg_tuple[0]
for pkg_tuple in RqtRoscommUtil.iterate_packages('launch')
])
self.package_combo_box.setItems.emit(packages)
# update topic_combo_box
#_, _, topic_types = rospy.get_master().getTopicTypes()
#self._topic_dict = dict(topic_types)
#self.topic_combo_box.setItems.emit(sorted(self._topic_dict.keys()))
@Slot()
def _update_finished(self):
self.package_combo_box.setEnabled(True)
self.file_combo_box.setEnabled(True)
@Slot(str)
def on_package_combo_box_currentIndexChanged(self, package):
#if topic_name in self._topic_dict:
# self.type_combo_box.setEditText(self._topic_dict[topic_name])
#pass
_launch_instance_list = RqtRoscommUtil.list_files(package, 'launch')
_launchfile_instances = [
x.split('/')[1] for x in _launch_instance_list
]
self.file_combo_box.setItems.emit(_launchfile_instances)
#self.file_combo_box.clear()
#self.file_combo_box.addItems(_launchfile_instances)
@Slot()
def on_add_launch_button_clicked(self):
package_name = str(self.package_combo_box.currentText())
file_name = str(self.file_combo_box.currentText())
arguments = str(self.arguments_combo_box.currentText())
enabled = False
self.add_launch.emit(package_name, file_name, arguments, enabled)
class BatteryProfileFrame(QFrame):
def __init__(self, parent=None):
super(BatteryProfileFrame, self).__init__(parent)
self._cmd_vel_topic_name = "/mobile_base/commands/velocity"
self._battery_topic_name = "/mobile_base/sensors/core/battery"
self._ui = Ui_battery_profile_frame()
self._motion = None
self.robot_state_subscriber = None
self._motion_thread = None
def setupUi(self, cmd_vel_topic_name):
self._ui.setupUi(self)
self._cmd_vel_topic_name = cmd_vel_topic_name
self._plot_layout = QVBoxLayout(self._ui.battery_profile_group_box)
self._plot_widget = PlotWidget()
self._plot_widget.setWindowTitle("Battery Profile")
self._plot_widget.topic_edit.setText(self._battery_topic_name)
self._plot_layout.addWidget(self._plot_widget)
self._plot_widget.switch_data_plot_widget(FullSizeDataPlot(self._plot_widget))
self._ui.start_button.setEnabled(True)
self._ui.stop_button.setEnabled(False)
def shutdown(self):
"""
Used to terminate the plugin
"""
rospy.loginfo("Kobuki TestSuite: battery test shutdown")
self._stop()
##########################################################################
# Widget Management
##########################################################################
def hibernate(self):
"""
This gets called when the frame goes out of focus (tab switch).
Disable everything to avoid running N tabs in parallel when in
reality we are only running one.
"""
self._stop()
def restore(self):
"""
Restore the frame after a hibernate.
"""
pass
##########################################################################
# Motion Callbacks
##########################################################################
def _run_finished(self):
self._ui.start_button.setEnabled(True)
self._ui.stop_button.setEnabled(False)
##########################################################################
# Qt Callbacks
##########################################################################
@Slot()
def on_start_button_clicked(self):
self._ui.start_button.setEnabled(False)
self._ui.stop_button.setEnabled(True)
if not self._motion:
self._motion = Rotate(self._cmd_vel_topic_name)
self._motion.init(self._ui.angular_speed_spinbox.value())
if not self.robot_state_subscriber:
self.robot_state_subscriber = rospy.Subscriber(
"/mobile_base/events/robot_state", RobotStateEvent, self.robot_state_callback
)
rospy.sleep(0.5)
self._plot_widget._start_time = rospy.get_time()
self._plot_widget.enable_timer(True)
try:
self._plot_widget.remove_topic(self._battery_topic_name)
except KeyError:
pass
self._plot_widget.add_topic(self._battery_topic_name)
self._motion_thread = WorkerThread(self._motion.execute, self._run_finished)
self._motion_thread.start()
@Slot()
def on_stop_button_clicked(self):
"""
Hardcore stoppage - straight to zero.
"""
self._stop()
def _stop(self):
self._plot_widget.enable_timer(False) # pause plot rendering
if self._motion_thread:
self._motion.stop()
self._motion_thread.wait()
self._motion_thread = None
if self._motion:
self._motion.shutdown()
self._motion = None
if self.robot_state_subscriber:
self.robot_state_subscriber.unregister()
self.robot_state_subscriber = None
self._ui.start_button.setEnabled(True)
self._ui.stop_button.setEnabled(False)
@pyqtSlot(float)
def on_angular_speed_spinbox_valueChanged(self, value):
if self._motion:
self._motion.init(self._ui.angular_speed_spinbox.value())
##########################################################################
# External Slot Callbacks
##########################################################################
@Slot(str)
def on_cmd_vel_topic_combo_box_currentIndexChanged(self, topic_name):
"""
To be connected to the configuration dock combo box (via the main testsuite frame)
"""
self._cmd_vel_topic_name = topic_name
print("DudetteBattery %s" % topic_name)
##########################################################################
# Ros Callbacks
##########################################################################
def robot_state_callback(self, data):
if data.state == RobotStateEvent.OFFLINE:
self.stop()
class CliffSensorFrame(QFrame):
STATE_FORWARD = "forward"
STATE_BACKWARD = "backward"
STATE_STOPPED = "stopped"
def __init__(self, parent=None):
super(CliffSensorFrame, self).__init__(parent)
self._ui = Ui_cliff_sensor_frame()
self._motion = TravelForward('/mobile_base/commands/velocity','/odom', '/mobile_base/events/cliff')
self._motion_thread = None
self._distance = 1.2
self._state = CliffSensorFrame.STATE_FORWARD
self._is_alive = False # Used to indicate whether the frame is alive or not (see hibernate/restore methods)
def setupUi(self):
self._ui.setupUi(self)
self._ui.start_button.setEnabled(True)
self._ui.stop_button.setEnabled(False)
self._motion.init(self._ui.speed_spinbox.value(), self._distance)
def shutdown(self):
'''
Used to terminate the plugin
'''
rospy.loginfo("Kobuki TestSuite: cliff sensor shutdown")
self._motion.shutdown()
##########################################################################
# Widget Management
##########################################################################
def hibernate(self):
'''
This gets called when the frame goes out of focus (tab switch).
Disable everything to avoid running N tabs in parallel when in
reality we are only running one.
'''
pass
def restore(self):
'''
Restore the frame after a hibernate.
'''
pass
##########################################################################
# Motion Callbacks
##########################################################################
def _run_finished(self):
if self._state == CliffSensorFrame.STATE_STOPPED:
return
elif self._state == CliffSensorFrame.STATE_FORWARD:
self._state = CliffSensorFrame.STATE_BACKWARD
self._motion.init(-self._motion.speed, 0.2)
self._motion_thread = WorkerThread(self._motion.execute, self._run_finished)
self._motion_thread.start()
else:
self._state = CliffSensorFrame.STATE_FORWARD
self._motion.init(-self._motion.speed, self._distance)
self._motion_thread = WorkerThread(self._motion.execute, self._run_finished)
self._motion_thread.start()
##########################################################################
# Qt Callbacks
##########################################################################
@Slot()
def on_start_button_clicked(self):
self._ui.start_button.setEnabled(False)
self._ui.stop_button.setEnabled(True)
self._state = CliffSensorFrame.STATE_FORWARD
self._motion_thread = WorkerThread(self._motion.execute, self._run_finished)
self._motion_thread.start()
@Slot()
def on_stop_button_clicked(self):
self._state = CliffSensorFrame.STATE_STOPPED
self.stop()
def stop(self):
self._motion.stop()
if self._motion_thread:
self._motion_thread.wait()
self._ui.start_button.setEnabled(True)
self._ui.stop_button.setEnabled(False)
@pyqtSlot(float)
def on_speed_spinbox_valueChanged(self, value):
self._motion.init(self._ui.speed_spinbox.value(), self._distance)
class FrameEditorGUI(ProjectPlugin, Interface):
signal_update = QtCore.Signal(int)
def __init__(self, context):
super(FrameEditorGUI, self).__init__(context)
self.setObjectName('FrameEditorGUI')
self.file_type = "YAML files(*.yaml)"
self.editor.parse_args(context.argv())
# Update filename display
self.update_current_filename()
## Update thread ##
##
self._update_thread.start()
self.update_all(3)
def create_editor(self):
editor = FrameEditor()
editor.observers.append(self)
self.signal_update.connect(self.update_all)
self._update_thread = WorkerThread(self._update_thread_run,
self._update_finished)
self.old_frame_list = []
self.old_selected = ""
return editor
def create_main_widget(self):
## Main widget
widget = QWidget()
ui_file = os.path.join(rospkg.RosPack().get_path('frame_editor'),
'src/frame_editor', 'FrameEditorGUI.ui')
loadUi(ui_file, widget)
widget.setObjectName('FrameEditorGUIUi')
#if context.serial_number() > 1:
# widget.setWindowTitle(widget.windowTitle() + (' (%d)' % context.serial_number()))
widget.setWindowTitle("frame editor")
## Undo View
#widget.undo_frame.layout().addWidget(QtWidgets.QUndoView(self.editor.undo_stack))
## Views
self.editor.init_views()
self.interface_style = FrameEditor_StyleWidget(self.editor)
widget.style_frame.layout().addWidget(
self.interface_style.get_widget())
## Connections ##
##
widget.btn_add.clicked.connect(self.btn_add_clicked)
widget.btn_delete.clicked.connect(self.btn_delete_clicked)
widget.btn_duplicate.clicked.connect(self.btn_duplicate_clicked)
widget.list_frames.currentTextChanged.connect(
self.selected_frame_changed)
widget.btn_refresh.clicked.connect(self.update_tf_list)
widget.btn_set_parent_rel.clicked.connect(
self.btn_set_parent_rel_clicked)
widget.btn_set_parent_abs.clicked.connect(
self.btn_set_parent_abs_clicked)
widget.btn_set_pose.clicked.connect(self.btn_set_pose_clicked)
widget.btn_set_position.clicked.connect(self.btn_set_position_clicked)
widget.btn_set_orientation.clicked.connect(
self.btn_set_orientation_clicked)
widget.btn_set_x.clicked.connect(self.btn_set_x_clicked)
widget.btn_set_y.clicked.connect(self.btn_set_y_clicked)
widget.btn_set_z.clicked.connect(self.btn_set_z_clicked)
widget.btn_set_a.clicked.connect(self.btn_set_a_clicked)
widget.btn_set_b.clicked.connect(self.btn_set_b_clicked)
widget.btn_set_c.clicked.connect(self.btn_set_c_clicked)
widget.btn_reset_position_rel.clicked.connect(
self.btn_reset_position_rel_clicked)
widget.btn_reset_position_abs.clicked.connect(
self.btn_reset_position_abs_clicked)
widget.btn_reset_orientation_rel.clicked.connect(
self.btn_reset_orientation_rel_clicked)
widget.btn_reset_orientation_abs.clicked.connect(
self.btn_reset_orientation_abs_clicked)
widget.txt_x.editingFinished.connect(self.x_valueChanged)
widget.txt_y.editingFinished.connect(self.y_valueChanged)
widget.txt_z.editingFinished.connect(self.z_valueChanged)
widget.txt_a.editingFinished.connect(self.a_valueChanged)
widget.txt_b.editingFinished.connect(self.b_valueChanged)
widget.txt_c.editingFinished.connect(self.c_valueChanged)
widget.btn_rad.toggled.connect(self.update_fields)
widget.combo_style.currentIndexChanged.connect(
self.frame_style_changed)
return widget
def _update_thread_run(self):
self.editor.run()
@Slot()
def _update_finished(self):
print "> Shutting down"
def update(self, editor, level, elements):
self.signal_update.emit(level)
@Slot(int)
def update_all(self, level):
## Update list widgets
if level & 1:
self.update_frame_list()
self.update_tf_list()
self.update_current_filename()
## Update the currently selected frame
if level & 2:
self.update_active_frame()
## Update only text fields, spin boxes,...
if level & 4:
self.update_fields()
@Slot()
def update_tf_list(self):
self.widget.list_tf.clear()
self.widget.list_tf.addItems(
sorted(self.editor.all_frame_ids(include_temp=False)))
def update_frame_list(self):
new_list = self.editor.frames.keys()
## Add missing
items = []
for item in new_list:
if item not in self.old_frame_list:
items.append(item)
self.widget.list_frames.addItems(items)
## Delete removed
for item in self.old_frame_list:
if item not in new_list:
if self.widget.list_frames.currentItem(
) and item == self.widget.list_frames.currentItem().text():
self.widget.list_frames.setCurrentItem(None)
found = self.widget.list_frames.findItems(
item, QtCore.Qt.MatchExactly)
self.widget.list_frames.takeItem(
self.widget.list_frames.row(found[0]))
self.widget.list_frames.sortItems()
self.old_frame_list = new_list
def update_active_frame(self):
if not self.editor.active_frame:
self.old_selected = ""
self.widget.list_frames.setCurrentItem(None)
self.widget.box_edit.setEnabled(False)
return # deselect and quit
self.widget.box_edit.setEnabled(True)
name = self.editor.active_frame.name
if name == self.old_selected:
return # no change
## Select item in list
items = self.widget.list_frames.findItems(name, QtCore.Qt.MatchExactly)
self.widget.list_frames.setCurrentItem(items[0])
self.update_fields()
self.old_selected = name
@Slot()
def update_fields(self):
f = self.editor.active_frame
if not f:
return
w = self.widget
w.txt_name.setText(f.name)
w.txt_parent.setText(f.parent)
## Relative
w.txt_x.setValue(f.position[0])
w.txt_y.setValue(f.position[1])
w.txt_z.setValue(f.position[2])
rot = tf.transformations.euler_from_quaternion(f.orientation)
if self.widget.btn_deg.isChecked():
rot = (180.0 * rot[0] / math.pi, 180.0 * rot[1] / math.pi,
180.0 * rot[2] / math.pi)
w.txt_a.setValue(rot[0])
w.txt_b.setValue(rot[1])
w.txt_c.setValue(rot[2])
txt_abs_pos = (w.txt_abs_x, w.txt_abs_y, w.txt_abs_z)
txt_abs_rot = (w.txt_abs_a, w.txt_abs_b, w.txt_abs_c)
## Absolute
try:
position, orientation = FromTransformStamped(
f.tf_buffer.lookup_transform('world', f.name, rospy.Time(0)))
for txt, p in zip(txt_abs_pos, position):
txt.setEnabled(True)
txt.setValue(p)
rot = tf.transformations.euler_from_quaternion(orientation)
if self.widget.btn_deg.isChecked():
rot = map(math.degrees, rot)
for txt, r in zip(txt_abs_rot, rot):
txt.setEnabled(True)
txt.setValue(r)
except:
for txt in txt_abs_rot + txt_abs_pos:
txt.setEnabled(False)
## Style
self.widget.combo_style.setCurrentIndex(
self.widget.combo_style.findText(f.style))
@Slot(str)
def selected_frame_changed(self, name):
if name == "":
return
if not self.editor.active_frame or (self.editor.active_frame.name !=
name):
self.editor.command(
Command_SelectElement(self.editor, self.editor.frames[name]))
## BUTTONS ##
##
def write_file(self, file_name):
return self.editor.save_file(file_name)
@Slot()
def clear_all(self):
self.editor.command(Command_ClearAll(self.editor))
@Slot(bool)
def btn_add_clicked(self, checked):
# Get a unique frame name
existing_frames = set(self.editor.all_frame_ids())
name, ok = QtWidgets.QInputDialog.getText(self.widget, "Add New Frame",
"Name:",
QtWidgets.QLineEdit.Normal,
"my_frame")
while ok and name in existing_frames:
name, ok = QtWidgets.QInputDialog.getText(
self.widget, "Add New Frame", "Name (must be unique):",
QtWidgets.QLineEdit.Normal, "my_frame")
if not ok:
return
if not existing_frames:
available_parents = ["world"]
else:
available_parents = self.editor.all_frame_ids(include_temp=False)
parent, ok = QtWidgets.QInputDialog.getItem(self.widget,
"Add New Frame",
"Parent Name:",
sorted(available_parents))
if not ok or parent == "":
return
self.editor.command(
Command_AddElement(self.editor, Frame(name, parent=parent)))
@Slot(bool)
def btn_duplicate_clicked(self, checked):
item = self.widget.list_frames.currentItem()
if not item:
return
source_name = item.text()
parent_name = self.editor.frames[source_name].parent
# Get a unique frame name
existing_frames = set(self.editor.all_frame_ids())
name, ok = QtWidgets.QInputDialog.getText(self.widget,
"Duplicate Frame", "Name:",
QtWidgets.QLineEdit.Normal,
source_name)
while ok and name in existing_frames:
name, ok = QtWidgets.QInputDialog.getText(
self.widget, "Duplicate Frame", "Name (must be unique):",
QtWidgets.QLineEdit.Normal, source_name)
if not ok:
return
self.editor.command(
Command_CopyElement(self.editor, name, source_name, parent_name))
@Slot(bool)
def btn_delete_clicked(self, checked):
item = self.widget.list_frames.currentItem()
if not item:
return
self.editor.command(
Command_RemoveElement(self.editor,
self.editor.frames[item.text()]))
## PARENTING ##
##
@Slot(bool)
def btn_set_parent_rel_clicked(self, checked):
self.set_parent(False)
@Slot(bool)
def btn_set_parent_abs_clicked(self, checked):
self.set_parent(True)
def set_parent(self, keep_absolute):
parent = self.widget.list_tf.currentItem()
if not parent:
return # none selected
if parent.text() == self.editor.active_frame.name:
return # you can't be your own parent
self.editor.command(
Command_SetParent(self.editor, self.editor.active_frame,
parent.text(), keep_absolute))
## SET BUTTONS ##
##
@Slot(bool)
def btn_set_pose_clicked(self, checked):
self.set_pose(["x", "y", "z", "a", "b", "c"])
@Slot(bool)
def btn_set_position_clicked(self, checked):
self.set_pose(["x", "y", "z"])
@Slot(bool)
def btn_set_orientation_clicked(self, checked):
self.set_pose(["a", "b", "c"])
@Slot(bool)
def btn_set_x_clicked(self, checked):
self.set_pose(["x"])
@Slot(bool)
def btn_set_y_clicked(self, checked):
self.set_pose(["y"])
@Slot(bool)
def btn_set_z_clicked(self, checked):
self.set_pose(["z"])
@Slot(bool)
def btn_set_a_clicked(self, checked):
self.set_pose(["a"])
@Slot(bool)
def btn_set_b_clicked(self, checked):
self.set_pose(["b"])
@Slot(bool)
def btn_set_c_clicked(self, checked):
self.set_pose(["c"])
def set_pose(self, mode):
source = self.widget.list_tf.currentItem()
if not source:
return # none selected
frame = self.editor.active_frame
self.editor.command(
Command_AlignElement(self.editor, frame, source.text(), mode))
## RESET BUTTONS ##
##
@Slot(bool)
def btn_reset_position_rel_clicked(self, checked):
self.editor.command(
Command_SetPosition(self.editor, self.editor.active_frame,
(0, 0, 0)))
@Slot(bool)
def btn_reset_position_abs_clicked(self, checked):
position, orientation = FromTransformStamped(
self.editor.active_frame.tf_buffer.lookup_transform(
self.editor.active_frame.parent, "world", rospy.Time(0)))
self.editor.command(
Command_SetPosition(self.editor, self.editor.active_frame,
position))
@Slot(bool)
def btn_reset_orientation_rel_clicked(self, checked):
self.editor.command(
Command_SetOrientation(self.editor, self.editor.active_frame,
(0, 0, 0, 1)))
@Slot(bool)
def btn_reset_orientation_abs_clicked(self, checked):
position, orientation = FromTransformStamped(
self.editor.active_frame.listener.lookupTransform(
self.editor.active_frame.parent, "world", rospy.Time(0)))
self.editor.command(
Command_SetOrientation(self.editor, self.editor.active_frame,
orientation))
## Spin Boxes ##
##
def set_value(self, widget, symbol):
frame = self.editor.active_frame
value = widget.value()
## Deg to rad
if self.widget.btn_deg.isChecked() and symbol in ['a', 'b', 'c']:
value = value * math.pi / 180.0
if frame.value(symbol) != value:
self.editor.command(
Command_SetValue(self.editor, self.editor.active_frame, symbol,
value))
@Slot()
def x_valueChanged(self):
self.set_value(self.widget.txt_x, 'x')
@Slot()
def y_valueChanged(self):
self.set_value(self.widget.txt_y, 'y')
@Slot()
def z_valueChanged(self):
self.set_value(self.widget.txt_z, 'z')
@Slot()
def a_valueChanged(self):
self.set_value(self.widget.txt_a, 'a')
@Slot()
def b_valueChanged(self):
self.set_value(self.widget.txt_b, 'b')
@Slot()
def c_valueChanged(self):
self.set_value(self.widget.txt_c, 'c')
## FRAME STYLE ##
##
@Slot(int)
def frame_style_changed(self, id):
style = self.widget.combo_style.currentText().lower()
if self.editor.active_frame.style != style:
self.editor.command(
Command_SetStyle(self.editor, self.editor.active_frame, style))
## PLUGIN ##
##
def shutdown_plugin(self):
super(FrameEditorGUI, self).shutdown_plugin()
self._update_thread.kill()
def __init__(self, context):
super(Conman, self).__init__(context)
self._dotcode_sub = None
self._topic_dict = {}
self._update_thread = WorkerThread(self._update_thread_run,
self._update_finished)
# Give QObjects reasonable names
self.setObjectName('Conman')
# 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._widget = QWidget()
# Get path to UI file which is a sibling of this file
# in this example the .ui and .py file are in the same folder
ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)),
'rqt_conman.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.ns_refresh_button.setIcon(QIcon.fromTheme('view-refresh'))
self._widget.setObjectName('ConmanPluginUi')
# 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)
palette = QPalette()
palette.setColor(QPalette.Background, Qt.white)
self._widget.setPalette(palette)
#self._widget.subscribe_button.setCheckable(True)
self._widget.namespace_input.currentIndexChanged.connect(
self._handle_refresh_clicked)
self._widget.ns_refresh_button.clicked.connect(self.refresh_combo_box)
self._widget.refresh_button.clicked[bool].connect(
self._handle_refresh_clicked)
self._widget.commit_button.clicked[bool].connect(
self._handle_commit_clicked)
#self._widget.xdot_widget.connect(
#self._widget.xdot_widget, SIGNAL('_update_graph'), self._widget.xdot_widget.set_dotcode)
self.update_graph_sig.connect(self._update_graph)
self.blocks = {}
self.groups = {}
self._ns = ""
self._actions_connected = False
self.enable_widgets(False)
self.new_dotcode_data = ''
self.update_timer = QTimer(self)
self.update_timer.setInterval(50)
self.update_timer.timeout.connect(self._update_widgets)
#self.update_timer.start()
self._get_blocks = None
self._set_blocks = None
self._blocks_model = QStandardItemModel(0, 4)
self._blocks_model.setHeaderData(0, Qt.Horizontal, "")
self._blocks_model.setHeaderData(1, Qt.Horizontal, "Action")
self._blocks_model.setHeaderData(2, Qt.Horizontal, "State")
self._blocks_model.setHeaderData(3, Qt.Horizontal, "Block")
self._widget.blocks_table.setModel(self._blocks_model)
self._blocks_delegate = BlocksDelegate(self)
self._widget.blocks_table.setItemDelegate(self._blocks_delegate)
self._blocks_model.itemChanged.connect(self.block_changed)
self._groups_model = QStandardItemModel(0, 4)
self._groups_model.setHeaderData(0, Qt.Horizontal, "")
self._groups_model.setHeaderData(1, Qt.Horizontal, "")
self._groups_model.setHeaderData(2, Qt.Horizontal, "")
self._groups_model.setHeaderData(3, Qt.Horizontal, "Group")
self._widget.groups_table.setModel(self._groups_model)
self._groups_delegate = GroupsDelegate(self)
self._widget.groups_table.setItemDelegate(self._groups_delegate)
self.refresh_combo_box()
def __init__(self, context):
super(Smach, self).__init__(context)
self.initialized = 0
self._dotcode_sub = None
self._topic_dict = {}
self._update_thread = WorkerThread(self._update_thread_run, self._update_finished)
# Give QObjects reasonable names
self.setObjectName('Smach')
# 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._widget = QWidget()
# Get path to UI file which is a sibling of this file
# in this example the .ui and .py file are in the same folder
ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'rqt_smach.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.ns_refresh_button.setIcon(QIcon.fromTheme('view-refresh'))
self._widget.setObjectName('SmachPluginUi')
# 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)
palette = QPalette ()
palette.setColor(QPalette.Background, Qt.white)
self._widget.setPalette(palette)
#Connect widgets with corresponding methods
self._widget.namespace_input.currentIndexChanged.connect(self._handle_ns_changed)
self._widget.ns_refresh_button.clicked.connect(self.refresh_combo_box)
self._widget.restrict_ns.clicked.connect(self.refresh_combo_box)
self._widget.ud_path_input.currentIndexChanged.connect(self._handle_ud_path)
self._widget.ud_set_initial.clicked.connect(self._handle_ud_set_path)
self._widget.ud_text_browser.setReadOnly(1)
self._widget.show_implicit_button.clicked.connect(self._handle_show_implicit)
self._widget.help_button.setIcon(QIcon.fromTheme('help-contents'))
self._widget.help_button.clicked.connect(self._handle_help)
self._widget.tree.clicked.connect(self._handle_tree_clicked)
#Depth and width spinners:
self._widget.depth_input.setRange(-1, 1337)
self._widget.depth_input.setValue(-1)
self._widget.depth_input.valueChanged.connect(self._set_depth)
self._widget.label_width_input.setRange(1, 1337)
self._widget.label_width_input.setValue(40)
self._widget.label_width_input.valueChanged.connect(self._set_width)
self._widget.tree.setColumnCount(1)
self._widget.tree.setHeaderLabels(["Containers"])
self._widget.tree.show()
self._ns = ""
self.refresh_combo_box()
# Bind path list
self._widget.path_input.currentIndexChanged.connect(
self._handle_path_changed)
#Keep Combo Boxes sorted
self._widget.namespace_input.setInsertPolicy(6)
self._widget.path_input.setInsertPolicy(6)
self._widget.ud_path_input.setInsertPolicy(6)
#Set up mouse actions for xdot widget
self._widget.xdot_widget.register_select_callback(self.select_cb)
# Create graph data structures
# Containers is a map of (container path) -> container proxy
self._containers = {}
self._top_containers = {}
# smach introspection client
self._client = smach_ros.IntrospectionClient()
self._selected_paths = []
# Message subscribers
self._structure_subs = {}
self._status_subs = {}
# Initialize xdot display state
self._path = '/'
self._needs_zoom = True
self._structure_changed = True
self._max_depth = -1
self._show_all_transitions = False
self._label_wrapper = textwrap.TextWrapper(40,break_long_words=True)
self._graph_needs_refresh = True
self._tree_needs_refresh = True
self._keep_running = True
self._update_server_list()
self._update_graph()
self._update_tree()
# Start a timer to update the server list
self._server_timer = QTimer(self)
self._server_timer.timeout.connect(self._update_server_list)
self._server_timer.start(1000)
# Start a timer to update the graph display
self._graph_timer = QTimer(self)
self._graph_timer.timeout.connect(self._update_graph)
self._graph_timer.start(1093)
# Start a timer to update the._widget.tree display
self._tree_timer = QTimer(self)
self._tree_timer.timeout.connect(self._update_tree)
self._tree_timer.start(1217)
class Conman(Plugin):
update_graph_sig = Signal(str)
def __init__(self, context):
super(Conman, self).__init__(context)
self._dotcode_sub = None
self._topic_dict = {}
self._update_thread = WorkerThread(self._update_thread_run, self._update_finished)
# Give QObjects reasonable names
self.setObjectName('Conman')
# 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._widget = QWidget()
# Get path to UI file which is a sibling of this file
# in this example the .ui and .py file are in the same folder
ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'rqt_conman.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.ns_refresh_button.setIcon(QIcon.fromTheme('view-refresh'))
self._widget.setObjectName('ConmanPluginUi')
# 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)
palette = QPalette ()
palette.setColor(QPalette.Background, Qt.white)
self._widget.setPalette(palette)
#self._widget.subscribe_button.setCheckable(True)
self._widget.namespace_input.currentIndexChanged.connect(self._handle_refresh_clicked)
self._widget.ns_refresh_button.clicked.connect(self.refresh_combo_box)
self._widget.refresh_button.clicked[bool].connect(self._handle_refresh_clicked)
self._widget.commit_button.clicked[bool].connect(self._handle_commit_clicked)
#self._widget.xdot_widget.connect(
#self._widget.xdot_widget, SIGNAL('_update_graph'), self._widget.xdot_widget.set_dotcode)
self.update_graph_sig.connect(self._update_graph)
self.blocks = { }
self.groups = { }
self._ns = ""
self._actions_connected = False
self.enable_widgets(False)
self.new_dotcode_data = ''
self.update_timer = QTimer(self)
self.update_timer.setInterval(50)
self.update_timer.timeout.connect(self._update_widgets)
#self.update_timer.start()
self._get_blocks = None
self._set_blocks = None
self._blocks_model = QStandardItemModel(0,4)
self._blocks_model.setHeaderData(0, Qt.Horizontal, "")
self._blocks_model.setHeaderData(1, Qt.Horizontal, "Action")
self._blocks_model.setHeaderData(2, Qt.Horizontal, "State")
self._blocks_model.setHeaderData(3, Qt.Horizontal, "Block")
self._widget.blocks_table.setModel(self._blocks_model)
self._blocks_delegate = BlocksDelegate(self)
self._widget.blocks_table.setItemDelegate(self._blocks_delegate)
self._blocks_model.itemChanged.connect(self.block_changed)
self._groups_model = QStandardItemModel(0,4)
self._groups_model.setHeaderData(0, Qt.Horizontal, "")
self._groups_model.setHeaderData(1, Qt.Horizontal, "")
self._groups_model.setHeaderData(2, Qt.Horizontal, "")
self._groups_model.setHeaderData(3, Qt.Horizontal, "Group")
self._widget.groups_table.setModel(self._groups_model)
self._groups_delegate = GroupsDelegate(self)
self._widget.groups_table.setItemDelegate(self._groups_delegate)
self.refresh_combo_box()
def block_changed(self, item):
row = item.row()
name = self._blocks_model.item(row,3).text()
block = self.blocks[name]
checked = item.checkState() == Qt.Checked
def shutdown_plugin(self):
# TODO unregister all publishers here
self._update_thread.kill()
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
# This will enable a setting button (gear icon) in each dock widget title bar
# Usually used to open a modal configuration dialog
@Slot()
def refresh_combo_box(self):
self._update_thread.kill()
self._widget.namespace_input.setEnabled(False)
self._widget.namespace_input.setEditText('updating...')
self._update_thread.start()
def _update_thread_run(self):
_, _, topic_types = rospy.get_master().getTopicTypes()
self._topic_dict = dict(topic_types)
keys = list(self._topic_dict.keys())
namespaces = list()
for i in keys:
if i.endswith("get_blocks_action/goal"):
namespaces.append(i[0:i.index("get_blocks_action/goal")])
self._widget.namespace_input.setItems.emit(namespaces)
@Slot()
def _update_finished(self):
self._widget.namespace_input.setEnabled(True)
def _get_result_cb(self, status, res):
rospy.loginfo("got result!")
self._blocks_model.setRowCount(0)
self._blocks_model.setRowCount(len(res.blocks))
for (i,block) in zip(range(len(res.blocks)),res.blocks):
# Store in dict
self.blocks[block.name] = block
cb = QStandardItem(True)
cb.setCheckable(True)
cb.setCheckState(Qt.Checked if block.state.value == conman_msgs.msg.TaskState.RUNNING else Qt.Unchecked)
cb.setTextAlignment(Qt.AlignHCenter)
cb.setTristate(True)
action = QStandardItem(True)
action.setText("")
state = QStandardItem(True)
state.setText(state_map[block.state.value])
name = QStandardItem(True)
name.setText(str(block.name))
self._blocks_model.setItem(i,0,cb)
self._blocks_model.setItem(i,1,action)
self._blocks_model.setItem(i,2,state)
self._blocks_model.setItem(i,3,name)
for (i,group) in zip(range(len(res.groups)),res.groups):
self.groups[group.name] = group
cb = QStandardItem(True)
cb.setCheckable(True)
cb.setCheckState(Qt.Checked)
cb.setTextAlignment(Qt.AlignHCenter)
cb.setEnabled(False)
name = QStandardItem(True)
name.setText(str(group.name))
self._groups_model.setItem(i,0,cb)
self._groups_model.setItem(i,3,name)
self._update_groups()
self._update_blocks()
self._widget.blocks_table.resizeColumnsToContents()
self._widget.blocks_table.horizontalHeader().setStretchLastSection(True)
self._widget.groups_table.resizeColumnsToContents()
self._widget.groups_table.horizontalHeader().setStretchLastSection(True)
def _update_blocks(self):
for (name, block) in self.blocks.items():
items = self._blocks_model.findItems(name, column=3)
if len(items) > 0:
item = items[0]
else:
continue
row = item.row()
checked = self._blocks_model.item(row,0).checkState() == Qt.Checked
if checked and block.state.value != conman_msgs.msg.TaskState.RUNNING:
self._blocks_model.item(row,1).setText("ENABLE")
elif not checked and block.state.value == conman_msgs.msg.TaskState.RUNNING:
self._blocks_model.item(row,1).setText("DISABLE")
else:
self._blocks_model.item(row,1).setText("")
self._update_groups()
def _enable_group(self, index, enable):
name = self._groups_model.item(index, 3).text()
group = self.groups[name]
for member in group.members:
items = self._blocks_model.findItems(member, column=3)
if len(items) > 0:
item = items[0]
else:
continue
row = item.row()
self._blocks_model.item(row,0).setCheckState(Qt.Checked if enable else Qt.Unchecked)
self._update_blocks()
def _update_groups(self):
for (name, group) in self.groups.items():
group_items = self._groups_model.findItems(name, column=3)
if len(group_items) > 0:
group_item = group_items[0]
else:
continue
group_row = group_item.row()
members_checked = []
for member in group.members:
items = self._blocks_model.findItems(member, column=3)
if len(items) > 0:
item = items[0]
else:
continue
row = item.row()
members_checked.append(self._blocks_model.item(row,0).checkState() == Qt.Checked)
if all(members_checked):
check = Qt.Checked
else:
check = Qt.Unchecked
self._groups_model.item(group_row,0).setCheckState(check)
def _query_blocks(self):
if self._get_blocks and self._actions_connected:
if self._get_blocks.simple_state == actionlib.SimpleGoalState.DONE:
rospy.loginfo("Getting blocks...")
goal = conman_msgs.msg.GetBlocksGoal()
goal.publish_flow_graph = self._widget.generate_graph_checkbox.isChecked()
self._get_blocks.send_goal(goal, done_cb=self._get_result_cb)
def enable_widgets(self, enable):
#self._widget.generate_graph_checkbox.setEnabled(enable)
self._widget.force_enable_checkbox.setEnabled(enable)
#self._widget.disable_unused_button.setEnabled(enable)
self._widget.xdot_widget.setEnabled(enable)
self._widget.blocks_table.setEnabled(enable)
self._widget.groups_table.setEnabled(enable)
self._widget.regenerate_graph_button.setEnabled(enable)
def _handle_refresh_clicked(self, checked):
ns = self._widget.namespace_input.currentText()
if len(ns) > 0:
if self._ns != ns:
self._actions_connected = False
self._ns = ns
self.enable_widgets(False)
self._dotcode_sub = rospy.Subscriber(
ns+'/dotcode',
std_msgs.msg.String,
self._dotcode_msg_cb)
self._get_blocks = actionlib.SimpleActionClient(
ns+'/get_blocks_action',
conman_msgs.msg.GetBlocksAction)
self._set_blocks = actionlib.SimpleActionClient(
ns+'/set_blocks_action',
conman_msgs.msg.SetBlocksAction)
rospy.loginfo("Creating action clients on namespace '%s'..." % ns)
if not self._get_blocks.wait_for_server(rospy.Duration(2)):
rospy.loginfo("Timed out waiting for %s." % self._get_blocks.action_client.ns)
return
if not self._set_blocks.wait_for_server(rospy.Duration(2)):
rospy.loginfo("Timed out waiting for %s." % self._set_blocks.action_client.ns)
return
rospy.loginfo("Action clients created.")
self._actions_connected = True
self.enable_widgets(True)
self._query_blocks()
def _handle_commit_clicked(self, checked):
if self._set_blocks and self._actions_connected:
if self._get_blocks.simple_state == actionlib.SimpleGoalState.DONE:
rospy.loginfo("Setting blocks...")
goal = conman_msgs.msg.SetBlocksGoal()
goal.diff = True
goal.force = True
for i in range(self._blocks_model.rowCount()):
name = self._blocks_model.item(i,3).text()
action = self._blocks_model.item(i,1).text()
if action == 'DISABLE':
goal.disable.append(name)
elif action == 'ENABLE':
goal.enable.append(name)
self._set_blocks.send_goal(goal, done_cb=self._get_set_result_cb)
def _get_set_result_cb(self, status, res):
self._query_blocks()
@Slot(str)
def _update_graph(self,dotcode):
global initialized
if initialized:
self._widget.xdot_widget.set_dotcode(dotcode, center=False);
else:
self._widget.xdot_widget.set_dotcode(dotcode, center=True);
self._widget.xdot_widget.zoom_to_fit()
initialized = 1
def _dotcode_msg_cb(self, msg):
#self.new_dotcode_data = msg.data
self.update_graph_sig.emit(msg.data)
def _update_widgets(self):
self._update_groups()
self._update_blocks()
class PayloadFrame(QFrame):
def __init__(self, parent=None):
super(PayloadFrame, self).__init__(parent)
self._gyro_topic_name = '/mobile_base/sensors/imu_data'
self._ui = Ui_payload_frame()
self._motion = None
self._motion_thread = None
def setupUi(self):
self._ui.setupUi(self)
self._ui.start_button.setEnabled(True)
self._ui.stop_button.setEnabled(False)
def shutdown(self):
'''
Used to terminate the plugin
'''
rospy.loginfo("Kobuki TestSuite: payload frame shutdown")
self._stop()
##########################################################################
# Widget Management
##########################################################################
def hibernate(self):
'''
This gets called when the frame goes out of focus (tab switch).
Disable everything to avoid running N tabs in parallel when in
reality we are only running one.
'''
self._stop()
def restore(self):
'''
Restore the frame after a hibernate.
'''
pass
##########################################################################
# Motion Callbacks
##########################################################################
def _run_finished(self):
self._motion_thread = None
self._motion = None
self._ui.start_button.setEnabled(True)
self._ui.stop_button.setEnabled(False)
##########################################################################
# Qt Callbacks
##########################################################################
@Slot()
def on_start_button_clicked(self):
self._ui.start_button.setEnabled(False)
self._ui.stop_button.setEnabled(True)
self._motion = Square('/mobile_base/commands/velocity', '/odom',
self._gyro_topic_name)
self._motion.init(self._ui.speed_spinbox.value(),
self._ui.distance_spinbox.value())
self._motion_thread = WorkerThread(self._motion.execute,
self._run_finished)
self._motion_thread.start()
@Slot()
def on_stop_button_clicked(self):
self._stop()
def _stop(self):
if self._motion:
self._motion.stop()
if self._motion_thread:
self._motion_thread.wait()
self._motion_thread = None
if self._motion:
self._motion = None
self._ui.start_button.setEnabled(True)
self._ui.stop_button.setEnabled(False)
@pyqtSlot(float)
def on_speed_spinbox_valueChanged(self, value):
if self._motion:
self._motion.init(self._ui.speed_spinbox.value(),
self._ui.distance_spinbox.value())
@pyqtSlot(float)
def on_distance_spinbox_valueChanged(self, value):
if self._motion:
self._motion.init(self._ui.speed_spinbox.value(),
self._ui.distance_spinbox.value())
def __init__(self, context):
super(RosPackGraph, self).__init__(context)
self.initialized = False
self._current_dotcode = None
self._update_thread = WorkerThread(self._update_thread_run, self._update_finished)
self._nodes = {}
self._edges = {}
self._options = {}
self._options_serialized = ''
self.setObjectName('RosPackGraph')
rospack = rospkg.RosPack()
rosstack = rospkg.RosStack()
# factory builds generic dotcode items
self.dotcode_factory = PydotFactory()
# self.dotcode_factory = PygraphvizFactory()
# generator builds rosgraph
self.dotcode_generator = RosPackageGraphDotcodeGenerator(rospack, rosstack)
# dot_to_qt transforms into Qt elements using dot layout
self.dot_to_qt = DotToQtGenerator()
self._widget = QWidget()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_dep'), 'resource', 'RosPackGraph.ui')
loadUi(ui_file, self._widget, {'InteractiveGraphicsView': InteractiveGraphicsView})
self._widget.setObjectName('RosPackGraphUi')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
self._scene = QGraphicsScene()
self._scene.setBackgroundBrush(Qt.white)
self._widget.graphics_view.setScene(self._scene)
self._widget.depth_combo_box.insertItem(0, self.tr('infinite'), -1)
self._widget.depth_combo_box.insertItem(1, self.tr('1'), 2)
self._widget.depth_combo_box.insertItem(2, self.tr('2'), 3)
self._widget.depth_combo_box.insertItem(3, self.tr('3'), 4)
self._widget.depth_combo_box.insertItem(4, self.tr('4'), 5)
self._widget.depth_combo_box.currentIndexChanged.connect(self._refresh_rospackgraph)
self._widget.directions_combo_box.insertItem(0, self.tr('depends'), 0)
self._widget.directions_combo_box.insertItem(1, self.tr('depends_on'), 1)
self._widget.directions_combo_box.insertItem(2, self.tr('both'), 2)
self._widget.directions_combo_box.currentIndexChanged.connect(self._refresh_rospackgraph)
self._widget.package_type_combo_box.insertItem(0, self.tr('wet & dry'), 3)
self._widget.package_type_combo_box.insertItem(1, self.tr('wet only'), 2)
self._widget.package_type_combo_box.insertItem(2, self.tr('dry only'), 1)
self._widget.package_type_combo_box.currentIndexChanged.connect(self._refresh_rospackgraph)
completionmodel = StackageCompletionModel(self._widget.filter_line_edit, rospack, rosstack)
completer = RepeatedWordCompleter(completionmodel, self)
completer.setCompletionMode(QCompleter.PopupCompletion)
completer.setWrapAround(True)
completer.setCaseSensitivity(Qt.CaseInsensitive)
self._widget.filter_line_edit.editingFinished.connect(self._refresh_rospackgraph)
self._widget.filter_line_edit.setCompleter(completer)
self._widget.filter_line_edit.selectionChanged.connect(self._clear_filter)
self._widget.with_stacks_check_box.clicked.connect(self._refresh_rospackgraph)
self._widget.mark_check_box.clicked.connect(self._refresh_rospackgraph)
self._widget.colorize_check_box.clicked.connect(self._refresh_rospackgraph)
self._widget.hide_transitives_check_box.clicked.connect(self._refresh_rospackgraph)
self._widget.refresh_graph_push_button.setIcon(QIcon.fromTheme('view-refresh'))
self._widget.refresh_graph_push_button.pressed.connect(self._update_rospackgraph)
self._widget.highlight_connections_check_box.toggled.connect(self._refresh_rospackgraph)
self._widget.auto_fit_graph_check_box.toggled.connect(self._refresh_rospackgraph)
self._widget.fit_in_view_push_button.setIcon(QIcon.fromTheme('zoom-original'))
self._widget.fit_in_view_push_button.pressed.connect(self._fit_in_view)
self._widget.load_dot_push_button.setIcon(QIcon.fromTheme('document-open'))
self._widget.load_dot_push_button.pressed.connect(self._load_dot)
self._widget.save_dot_push_button.setIcon(QIcon.fromTheme('document-save-as'))
self._widget.save_dot_push_button.pressed.connect(self._save_dot)
self._widget.save_as_svg_push_button.setIcon(QIcon.fromTheme('document-save-as'))
self._widget.save_as_svg_push_button.pressed.connect(self._save_svg)
self._widget.save_as_image_push_button.setIcon(QIcon.fromTheme('image'))
self._widget.save_as_image_push_button.pressed.connect(self._save_image)
self._deferred_fit_in_view.connect(self._fit_in_view, Qt.QueuedConnection)
self._deferred_fit_in_view.emit()
context.add_widget(self._widget)
# If in either of following case, this turnes True
# - 1st filtering key is already input by user
# - filtering key is restored
self._filtering_started = False
class RosPackGraph(Plugin):
_deferred_fit_in_view = Signal()
def __init__(self, context):
super(RosPackGraph, self).__init__(context)
self.initialized = False
self._current_dotcode = None
self._update_thread = WorkerThread(self._update_thread_run, self._update_finished)
self._nodes = {}
self._edges = {}
self._options = {}
self._options_serialized = ''
self.setObjectName('RosPackGraph')
rospack = rospkg.RosPack()
rosstack = rospkg.RosStack()
# factory builds generic dotcode items
self.dotcode_factory = PydotFactory()
# self.dotcode_factory = PygraphvizFactory()
# generator builds rosgraph
self.dotcode_generator = RosPackageGraphDotcodeGenerator(rospack, rosstack)
# dot_to_qt transforms into Qt elements using dot layout
self.dot_to_qt = DotToQtGenerator()
self._widget = QWidget()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_dep'), 'resource', 'RosPackGraph.ui')
loadUi(ui_file, self._widget, {'InteractiveGraphicsView': InteractiveGraphicsView})
self._widget.setObjectName('RosPackGraphUi')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
self._scene = QGraphicsScene()
self._scene.setBackgroundBrush(Qt.white)
self._widget.graphics_view.setScene(self._scene)
self._widget.depth_combo_box.insertItem(0, self.tr('infinite'), -1)
self._widget.depth_combo_box.insertItem(1, self.tr('1'), 2)
self._widget.depth_combo_box.insertItem(2, self.tr('2'), 3)
self._widget.depth_combo_box.insertItem(3, self.tr('3'), 4)
self._widget.depth_combo_box.insertItem(4, self.tr('4'), 5)
self._widget.depth_combo_box.currentIndexChanged.connect(self._refresh_rospackgraph)
self._widget.directions_combo_box.insertItem(0, self.tr('depends'), 0)
self._widget.directions_combo_box.insertItem(1, self.tr('depends_on'), 1)
self._widget.directions_combo_box.insertItem(2, self.tr('both'), 2)
self._widget.directions_combo_box.currentIndexChanged.connect(self._refresh_rospackgraph)
self._widget.package_type_combo_box.insertItem(0, self.tr('wet & dry'), 3)
self._widget.package_type_combo_box.insertItem(1, self.tr('wet only'), 2)
self._widget.package_type_combo_box.insertItem(2, self.tr('dry only'), 1)
self._widget.package_type_combo_box.currentIndexChanged.connect(self._refresh_rospackgraph)
completionmodel = StackageCompletionModel(self._widget.filter_line_edit, rospack, rosstack)
completer = RepeatedWordCompleter(completionmodel, self)
completer.setCompletionMode(QCompleter.PopupCompletion)
completer.setWrapAround(True)
completer.setCaseSensitivity(Qt.CaseInsensitive)
self._widget.filter_line_edit.editingFinished.connect(self._refresh_rospackgraph)
self._widget.filter_line_edit.setCompleter(completer)
self._widget.filter_line_edit.selectionChanged.connect(self._clear_filter)
self._widget.with_stacks_check_box.clicked.connect(self._refresh_rospackgraph)
self._widget.mark_check_box.clicked.connect(self._refresh_rospackgraph)
self._widget.colorize_check_box.clicked.connect(self._refresh_rospackgraph)
self._widget.hide_transitives_check_box.clicked.connect(self._refresh_rospackgraph)
self._widget.refresh_graph_push_button.setIcon(QIcon.fromTheme('view-refresh'))
self._widget.refresh_graph_push_button.pressed.connect(self._update_rospackgraph)
self._widget.highlight_connections_check_box.toggled.connect(self._refresh_rospackgraph)
self._widget.auto_fit_graph_check_box.toggled.connect(self._refresh_rospackgraph)
self._widget.fit_in_view_push_button.setIcon(QIcon.fromTheme('zoom-original'))
self._widget.fit_in_view_push_button.pressed.connect(self._fit_in_view)
self._widget.load_dot_push_button.setIcon(QIcon.fromTheme('document-open'))
self._widget.load_dot_push_button.pressed.connect(self._load_dot)
self._widget.save_dot_push_button.setIcon(QIcon.fromTheme('document-save-as'))
self._widget.save_dot_push_button.pressed.connect(self._save_dot)
self._widget.save_as_svg_push_button.setIcon(QIcon.fromTheme('document-save-as'))
self._widget.save_as_svg_push_button.pressed.connect(self._save_svg)
self._widget.save_as_image_push_button.setIcon(QIcon.fromTheme('image'))
self._widget.save_as_image_push_button.pressed.connect(self._save_image)
self._deferred_fit_in_view.connect(self._fit_in_view, Qt.QueuedConnection)
self._deferred_fit_in_view.emit()
context.add_widget(self._widget)
# If in either of following case, this turnes True
# - 1st filtering key is already input by user
# - filtering key is restored
self._filtering_started = False
def shutdown_plugin(self):
self._update_thread.kill()
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value('depth_combo_box_index', self._widget.depth_combo_box.currentIndex())
instance_settings.set_value('directions_combo_box_index', self._widget.directions_combo_box.currentIndex())
instance_settings.set_value('package_type_combo_box', self._widget.package_type_combo_box.currentIndex())
instance_settings.set_value('filter_line_edit_text', self._widget.filter_line_edit.text())
instance_settings.set_value('with_stacks_state', self._widget.with_stacks_check_box.isChecked())
instance_settings.set_value('hide_transitives_state', self._widget.hide_transitives_check_box.isChecked())
instance_settings.set_value('mark_state', self._widget.mark_check_box.isChecked())
instance_settings.set_value('colorize_state', self._widget.colorize_check_box.isChecked())
instance_settings.set_value('auto_fit_graph_check_box_state', self._widget.auto_fit_graph_check_box.isChecked())
instance_settings.set_value('highlight_connections_check_box_state', self._widget.highlight_connections_check_box.isChecked())
def restore_settings(self, plugin_settings, instance_settings):
_str_filter = instance_settings.value('filter_line_edit_text', '')
if (_str_filter == None or _str_filter == '') and \
not self._filtering_started:
_str_filter = '(Separate pkgs by comma)'
else:
self._filtering_started = True
self._widget.depth_combo_box.setCurrentIndex(int(instance_settings.value('depth_combo_box_index', 0)))
self._widget.directions_combo_box.setCurrentIndex(int(instance_settings.value('directions_combo_box_index', 0)))
self._widget.package_type_combo_box.setCurrentIndex(int(instance_settings.value('package_type_combo_box', 0)))
self._widget.filter_line_edit.setText(_str_filter)
self._widget.with_stacks_check_box.setChecked(instance_settings.value('with_stacks_state', True) in [True, 'true'])
self._widget.mark_check_box.setChecked(instance_settings.value('mark_state', True) in [True, 'true'])
self._widget.colorize_check_box.setChecked(instance_settings.value('colorize_state', False) in [True, 'true'])
self._widget.hide_transitives_check_box.setChecked(instance_settings.value('hide_transitives_state', False) in [True, 'true'])
self._widget.auto_fit_graph_check_box.setChecked(instance_settings.value('auto_fit_graph_check_box_state', True) in [True, 'true'])
self._widget.highlight_connections_check_box.setChecked(instance_settings.value('highlight_connections_check_box_state', True) in [True, 'true'])
self.initialized = True
self._refresh_rospackgraph()
def _update_rospackgraph(self):
# re-enable controls customizing fetched ROS graph
self._widget.depth_combo_box.setEnabled(True)
self._widget.directions_combo_box.setEnabled(True)
self._widget.package_type_combo_box.setEnabled(True)
self._widget.filter_line_edit.setEnabled(True)
self._widget.with_stacks_check_box.setEnabled(True)
self._widget.mark_check_box.setEnabled(True)
self._widget.colorize_check_box.setEnabled(True)
self._widget.hide_transitives_check_box.setEnabled(True)
self._refresh_rospackgraph(force_update=True)
def _update_options(self):
self._options['depth'] = self._widget.depth_combo_box.itemData(self._widget.depth_combo_box.currentIndex())
self._options['directions'] = self._widget.directions_combo_box.itemData(self._widget.directions_combo_box.currentIndex())
self._options['package_types'] = self._widget.package_type_combo_box.itemData(self._widget.package_type_combo_box.currentIndex())
self._options['with_stacks'] = self._widget.with_stacks_check_box.isChecked()
self._options['mark_selected'] = self._widget.mark_check_box.isChecked()
self._options['hide_transitives'] = self._widget.hide_transitives_check_box.isChecked()
# TODO: Allow different color themes
self._options['colortheme'] = True if self._widget.colorize_check_box.isChecked() else None
self._options['names'] = self._widget.filter_line_edit.text().split(',')
if self._options['names'] == [u'None']:
self._options['names'] = []
self._options['highlight_level'] = 3 if self._widget.highlight_connections_check_box.isChecked() else 1
self._options['auto_fit'] = self._widget.auto_fit_graph_check_box.isChecked()
def _refresh_rospackgraph(self, force_update=False):
if not self.initialized:
return
self._update_thread.kill()
self._update_options()
# avoid update if options did not change and force_update is not set
new_options_serialized = pickle.dumps(self._options)
if new_options_serialized == self._options_serialized and not force_update:
return
self._options_serialized = pickle.dumps(self._options)
self._scene.setBackgroundBrush(Qt.lightGray)
self._update_thread.start()
# this runs in a non-gui thread, so don't access widgets here directly
def _update_thread_run(self):
self._update_graph(self._generate_dotcode())
@Slot()
def _update_finished(self):
self._scene.setBackgroundBrush(Qt.white)
self._redraw_graph_scene()
# this runs in a non-gui thread, so don't access widgets here directly
def _generate_dotcode(self):
includes = []
excludes = []
for name in self._options['names']:
if name.strip().startswith('-'):
excludes.append(name.strip()[1:])
else:
includes.append(name.strip())
# orientation = 'LR'
descendants = True
ancestors = True
if self._options['directions'] == 1:
descendants = False
if self._options['directions'] == 0:
ancestors = False
return self.dotcode_generator.generate_dotcode(dotcode_factory=self.dotcode_factory,
selected_names=includes,
excludes=excludes,
depth=self._options['depth'],
with_stacks=self._options['with_stacks'],
descendants=descendants,
ancestors=ancestors,
mark_selected=self._options['mark_selected'],
colortheme=self._options['colortheme'],
hide_transitives=self._options['hide_transitives'],
hide_wet=self._options['package_types'] == 1,
hide_dry=self._options['package_types'] == 2)
# this runs in a non-gui thread, so don't access widgets here directly
def _update_graph(self, dotcode):
self._current_dotcode = dotcode
self._nodes, self._edges = self.dot_to_qt.dotcode_to_qt_items(self._current_dotcode, self._options['highlight_level'])
def _generate_tool_tip(self, url):
if url is not None and ':' in url:
item_type, item_path = url.split(':', 1)
if item_type == 'node':
tool_tip = 'Node:\n %s' % (item_path)
service_names = rosservice.get_service_list(node=item_path)
if service_names:
tool_tip += '\nServices:'
for service_name in service_names:
try:
service_type = rosservice.get_service_type(service_name)
tool_tip += '\n %s [%s]' % (service_name, service_type)
except rosservice.ROSServiceIOException, e:
tool_tip += '\n %s' % (e)
return tool_tip
elif item_type == 'topic':
topic_type, topic_name, _ = rostopic.get_topic_type(item_path)
return 'Topic:\n %s\nType:\n %s' % (topic_name, topic_type)
return url
def __init__(self, context):
super(Conman, self).__init__(context)
self._dotcode_sub = None
self._topic_dict = {}
self._update_thread = WorkerThread(self._update_thread_run, self._update_finished)
# Give QObjects reasonable names
self.setObjectName('Conman')
# 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._widget = QWidget()
# Get path to UI file which is a sibling of this file
# in this example the .ui and .py file are in the same folder
ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'rqt_conman.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.ns_refresh_button.setIcon(QIcon.fromTheme('view-refresh'))
self._widget.setObjectName('ConmanPluginUi')
# 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)
palette = QPalette ()
palette.setColor(QPalette.Background, Qt.white)
self._widget.setPalette(palette)
#self._widget.subscribe_button.setCheckable(True)
self._widget.namespace_input.currentIndexChanged.connect(self._handle_refresh_clicked)
self._widget.ns_refresh_button.clicked.connect(self.refresh_combo_box)
self._widget.refresh_button.clicked[bool].connect(self._handle_refresh_clicked)
self._widget.commit_button.clicked[bool].connect(self._handle_commit_clicked)
#self._widget.xdot_widget.connect(
#self._widget.xdot_widget, SIGNAL('_update_graph'), self._widget.xdot_widget.set_dotcode)
self.update_graph_sig.connect(self._update_graph)
self.blocks = { }
self.groups = { }
self._ns = ""
self._actions_connected = False
self.enable_widgets(False)
self.new_dotcode_data = ''
self.update_timer = QTimer(self)
self.update_timer.setInterval(50)
self.update_timer.timeout.connect(self._update_widgets)
#self.update_timer.start()
self._get_blocks = None
self._set_blocks = None
self._blocks_model = QStandardItemModel(0,4)
self._blocks_model.setHeaderData(0, Qt.Horizontal, "")
self._blocks_model.setHeaderData(1, Qt.Horizontal, "Action")
self._blocks_model.setHeaderData(2, Qt.Horizontal, "State")
self._blocks_model.setHeaderData(3, Qt.Horizontal, "Block")
self._widget.blocks_table.setModel(self._blocks_model)
self._blocks_delegate = BlocksDelegate(self)
self._widget.blocks_table.setItemDelegate(self._blocks_delegate)
self._blocks_model.itemChanged.connect(self.block_changed)
self._groups_model = QStandardItemModel(0,4)
self._groups_model.setHeaderData(0, Qt.Horizontal, "")
self._groups_model.setHeaderData(1, Qt.Horizontal, "")
self._groups_model.setHeaderData(2, Qt.Horizontal, "")
self._groups_model.setHeaderData(3, Qt.Horizontal, "Group")
self._widget.groups_table.setModel(self._groups_model)
self._groups_delegate = GroupsDelegate(self)
self._widget.groups_table.setItemDelegate(self._groups_delegate)
self.refresh_combo_box()
class RosPackGraph(Plugin):
_deferred_fit_in_view = Signal()
def __init__(self, context):
super(RosPackGraph, self).__init__(context)
self.initialized = False
self._current_dotcode = None
self._update_thread = WorkerThread(self._update_thread_run,
self._update_finished)
self._nodes = []
self._edges = []
self._options = {}
self._options_serialized = ''
self.setObjectName('RosPackGraph')
rospack = rospkg.RosPack()
rosstack = rospkg.RosStack()
# factory builds generic dotcode items
self.dotcode_factory = PydotFactory()
# self.dotcode_factory = PygraphvizFactory()
# generator builds rosgraph
self.dotcode_generator = RosPackageGraphDotcodeGenerator(
rospack, rosstack)
# dot_to_qt transforms into Qt elements using dot layout
self.dot_to_qt = DotToQtGenerator()
self._widget = QWidget()
ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)),
'RosPackGraph.ui')
loadUi(ui_file, self._widget,
{'InteractiveGraphicsView': InteractiveGraphicsView})
self._widget.setObjectName('RosPackGraphUi')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
self._scene = QGraphicsScene()
self._scene.setBackgroundBrush(Qt.white)
self._widget.graphics_view.setScene(self._scene)
self._widget.depth_combo_box.insertItem(0, self.tr('infinite'), -1)
self._widget.depth_combo_box.insertItem(1, self.tr('1'), 2)
self._widget.depth_combo_box.insertItem(2, self.tr('2'), 3)
self._widget.depth_combo_box.insertItem(3, self.tr('3'), 4)
self._widget.depth_combo_box.insertItem(4, self.tr('4'), 5)
self._widget.depth_combo_box.setCurrentIndex(0)
self._widget.depth_combo_box.currentIndexChanged.connect(
self._refresh_rospackgraph)
self._widget.directions_combo_box.insertItem(0, self.tr('depends'), 0)
self._widget.directions_combo_box.insertItem(1, self.tr('depends_on'),
1)
self._widget.directions_combo_box.insertItem(2, self.tr('both'), 2)
self._widget.directions_combo_box.setCurrentIndex(2)
self._widget.directions_combo_box.currentIndexChanged.connect(
self._refresh_rospackgraph)
completionmodel = StackageCompletionModel(
self._widget.filter_line_edit, rospack, rosstack)
completer = RepeatedWordCompleter(completionmodel, self)
completer.setCompletionMode(QCompleter.PopupCompletion)
completer.setWrapAround(True)
completer.setCaseSensitivity(Qt.CaseInsensitive)
self._widget.filter_line_edit.editingFinished.connect(
self._refresh_rospackgraph)
self._widget.filter_line_edit.setCompleter(completer)
self._widget.with_stacks_check_box.clicked.connect(
self._refresh_rospackgraph)
self._widget.mark_check_box.clicked.connect(self._refresh_rospackgraph)
self._widget.colorize_check_box.clicked.connect(
self._refresh_rospackgraph)
self._widget.hide_transitives_check_box.clicked.connect(
self._refresh_rospackgraph)
self._widget.refresh_graph_push_button.setIcon(
QIcon.fromTheme('view-refresh'))
self._widget.refresh_graph_push_button.pressed.connect(
self._update_rospackgraph)
self._widget.highlight_connections_check_box.toggled.connect(
self._refresh_rospackgraph)
self._widget.auto_fit_graph_check_box.toggled.connect(
self._refresh_rospackgraph)
self._widget.fit_in_view_push_button.setIcon(
QIcon.fromTheme('zoom-original'))
self._widget.fit_in_view_push_button.pressed.connect(self._fit_in_view)
self._widget.load_dot_push_button.setIcon(
QIcon.fromTheme('document-open'))
self._widget.load_dot_push_button.pressed.connect(self._load_dot)
self._widget.save_dot_push_button.setIcon(
QIcon.fromTheme('document-save-as'))
self._widget.save_dot_push_button.pressed.connect(self._save_dot)
self._widget.save_as_svg_push_button.setIcon(
QIcon.fromTheme('document-save-as'))
self._widget.save_as_svg_push_button.pressed.connect(self._save_svg)
self._widget.save_as_image_push_button.setIcon(
QIcon.fromTheme('image'))
self._widget.save_as_image_push_button.pressed.connect(
self._save_image)
self._deferred_fit_in_view.connect(self._fit_in_view,
Qt.QueuedConnection)
self._deferred_fit_in_view.emit()
context.add_widget(self._widget)
def shutdown_plugin(self):
self._update_thread.kill()
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value(
'depth_combo_box_index',
self._widget.depth_combo_box.currentIndex())
instance_settings.set_value(
'directions_combo_box_index',
self._widget.directions_combo_box.currentIndex())
instance_settings.set_value('filter_line_edit_text',
self._widget.filter_line_edit.text())
instance_settings.set_value(
'with_stacks_state',
self._widget.with_stacks_check_box.isChecked())
instance_settings.set_value(
'hide_transitives_state',
self._widget.hide_transitives_check_box.isChecked())
instance_settings.set_value('mark_state',
self._widget.mark_check_box.isChecked())
instance_settings.set_value(
'colorize_state', self._widget.colorize_check_box.isChecked())
instance_settings.set_value(
'auto_fit_graph_check_box_state',
self._widget.auto_fit_graph_check_box.isChecked())
instance_settings.set_value(
'highlight_connections_check_box_state',
self._widget.highlight_connections_check_box.isChecked())
def restore_settings(self, plugin_settings, instance_settings):
self._widget.depth_combo_box.setCurrentIndex(
int(instance_settings.value('depth_combo_box_index', 0)))
self._widget.directions_combo_box.setCurrentIndex(
int(instance_settings.value('directions_combo_box_index', 0)))
self._widget.filter_line_edit.setText(
instance_settings.value('filter_line_edit_text', ''))
self._widget.with_stacks_check_box.setChecked(
instance_settings.value('with_stacks_state', True) in
[True, 'true'])
self._widget.mark_check_box.setChecked(
instance_settings.value('mark_state', True) in [True, 'true'])
self._widget.colorize_check_box.setChecked(
instance_settings.value('colorize_state', True) in [True, 'true'])
self._widget.hide_transitives_check_box.setChecked(
instance_settings.value('hide_transitives_state', True) in
[True, 'true'])
self._widget.auto_fit_graph_check_box.setChecked(
instance_settings.value('auto_fit_graph_check_box_state', True) in
[True, 'true'])
self._widget.highlight_connections_check_box.setChecked(
instance_settings.value('highlight_connections_check_box_state',
True) in [True, 'true'])
self.initialized = True
self._refresh_rospackgraph()
def _update_rospackgraph(self):
# re-enable controls customizing fetched ROS graph
self._widget.depth_combo_box.setEnabled(True)
self._widget.directions_combo_box.setEnabled(True)
self._widget.filter_line_edit.setEnabled(True)
self._widget.with_stacks_check_box.setEnabled(True)
self._widget.mark_check_box.setEnabled(True)
self._widget.colorize_check_box.setEnabled(True)
self._widget.hide_transitives_check_box.setEnabled(True)
self._refresh_rospackgraph(force_update=True)
def _update_options(self):
self._options['depth'] = self._widget.depth_combo_box.itemData(
self._widget.depth_combo_box.currentIndex())
self._options[
'directions'] = self._widget.directions_combo_box.itemData(
self._widget.directions_combo_box.currentIndex())
self._options[
'with_stacks'] = self._widget.with_stacks_check_box.isChecked()
self._options['mark_selected'] = self._widget.mark_check_box.isChecked(
)
self._options[
'hide_transitives'] = self._widget.hide_transitives_check_box.isChecked(
)
# TODO: Allow different color themes
self._options[
'colortheme'] = True if self._widget.colorize_check_box.isChecked(
) else None
self._options['names'] = self._widget.filter_line_edit.text().split(
',')
if self._options['names'] == [u'None']:
self._options['names'] = []
self._options[
'highlight_level'] = 3 if self._widget.highlight_connections_check_box.isChecked(
) else 1
self._options[
'auto_fit'] = self._widget.auto_fit_graph_check_box.isChecked()
def _refresh_rospackgraph(self, force_update=False):
if not self.initialized:
return
self._update_thread.kill()
self._update_options()
# avoid update if options did not change and force_update is not set
new_options_serialized = pickle.dumps(self._options)
if new_options_serialized == self._options_serialized and not force_update:
return
self._options_serialized = pickle.dumps(self._options)
self._scene.setBackgroundBrush(Qt.lightGray)
self._update_thread.start()
# this runs in a non-gui thread, so don't access widgets here directly
def _update_thread_run(self):
self._update_graph(self._generate_dotcode())
@Slot()
def _update_finished(self):
self._scene.setBackgroundBrush(Qt.white)
self._redraw_graph_scene()
# this runs in a non-gui thread, so don't access widgets here directly
def _generate_dotcode(self):
includes = []
excludes = []
for name in self._options['names']:
if name.strip().startswith('-'):
excludes.append(name.strip()[1:])
else:
includes.append(name.strip())
# orientation = 'LR'
descendants = True
ancestors = True
if self._options['directions'] == 1:
descendants = False
if self._options['directions'] == 0:
ancestors = False
return self.dotcode_generator.generate_dotcode(
dotcode_factory=self.dotcode_factory,
selected_names=includes,
excludes=excludes,
depth=self._options['depth'],
with_stacks=self._options['with_stacks'],
descendants=descendants,
ancestors=ancestors,
mark_selected=self._options['mark_selected'],
colortheme=self._options['colortheme'],
hide_transitives=self._options['hide_transitives'])
# this runs in a non-gui thread, so don't access widgets here directly
def _update_graph(self, dotcode):
self._current_dotcode = dotcode
self._nodes, self._edges = self.dot_to_qt.dotcode_to_qt_items(
self._current_dotcode, self._options['highlight_level'])
def _generate_tool_tip(self, url):
if url is not None and ':' in url:
item_type, item_path = url.split(':', 1)
if item_type == 'node':
tool_tip = 'Node:\n %s' % (item_path)
service_names = rosservice.get_service_list(node=item_path)
if service_names:
tool_tip += '\nServices:'
for service_name in service_names:
try:
service_type = rosservice.get_service_type(
service_name)
tool_tip += '\n %s [%s]' % (service_name,
service_type)
except rosservice.ROSServiceIOException, e:
tool_tip += '\n %s' % (e)
return tool_tip
elif item_type == 'topic':
topic_type, topic_name, _ = rostopic.get_topic_type(item_path)
return 'Topic:\n %s\nType:\n %s' % (topic_name, topic_type)
return url
class PublisherWidget(QWidget):
add_publisher = Signal(str, str, float, bool)
change_publisher = Signal(int, str, str, str, object)
publish_once = Signal(int)
remove_publisher = Signal(int)
clean_up_publishers = Signal()
def __init__(self, node, parent=None):
super(PublisherWidget, self).__init__(parent)
self._node = node
self._topic_dict = {}
self._update_thread = WorkerThread(self._update_thread_run,
self._update_finished)
package_path = get_package_path('rqt_publisher')
ui_file = os.path.join(package_path, 'share', 'rqt_publisher',
'resource', 'Publisher.ui')
loadUi(
ui_file, self, {
'ExtendedComboBox': ExtendedComboBox,
'PublisherTreeWidget': PublisherTreeWidget
})
self.refresh_button.setIcon(QIcon.fromTheme('view-refresh'))
self.refresh_button.clicked.connect(self.refresh_combo_boxes)
self.add_publisher_button.setIcon(QIcon.fromTheme('list-add'))
self.remove_publisher_button.setIcon(QIcon.fromTheme('list-remove'))
self.clear_button.setIcon(QIcon.fromTheme('edit-clear'))
self.refresh_combo_boxes()
self.publisher_tree_widget.model().item_value_changed.connect(
self.change_publisher)
self.publisher_tree_widget.remove_publisher.connect(
self.remove_publisher)
self.publisher_tree_widget.publish_once.connect(self.publish_once)
self.remove_publisher_button.clicked.connect(
self.publisher_tree_widget.remove_selected_publishers)
self.clear_button.clicked.connect(self.clean_up_publishers)
def shutdown_plugin(self):
self._update_thread.kill()
@Slot()
def refresh_combo_boxes(self):
self._update_thread.kill()
self.type_combo_box.setEnabled(False)
self.topic_combo_box.setEnabled(False)
self.type_combo_box.setEditText('updating...')
self.topic_combo_box.setEditText('updating...')
self._update_thread.start()
def _get_message_types(self, package_name):
"""
Implementation taken from ros2cli.
https://github.com/ros2/ros2cli/blob/master/ros2msg/ros2msg/api/__init__.py
"""
if not has_resource('packages', package_name):
raise LookupError('Unknown package name "{}"'.format(package_name))
try:
content, _ = get_resource('rosidl_interfaces', package_name)
except LookupError:
return []
interface_names = content.splitlines()
# TODO(dirk-thomas) this logic should come from a rosidl related package
# Only return messages in msg folder
return [
n[4:-4] for n in interface_names
if n.startswith('msg/') and n.endswith('.msg')
]
# this runs in a non-gui thread, so don't access widgets here directly
def _update_thread_run(self):
# update type_combo_box
message_type_names = []
message_types = get_all_message_types()
for package, message_types in message_types.items():
for message_type in message_types:
base_type_str = package + '/msg/' + message_type
message_class = get_message_class(base_type_str)
if message_class is not None:
message_type_names.append(base_type_str)
self.type_combo_box.setItems.emit(sorted(message_type_names))
# update topic_combo_box
topic_names_and_types = self._node.get_topic_names_and_types()
self._topic_dict = dict(topic_names_and_types)
self.topic_combo_box.setItems.emit(sorted(self._topic_dict.keys()))
@Slot()
def _update_finished(self):
self.type_combo_box.setEnabled(True)
self.topic_combo_box.setEnabled(True)
@Slot(str)
def on_topic_combo_box_currentIndexChanged(self, topic_name):
# More than one topic type is possible per topic, this recommends the first one
if topic_name in self._topic_dict and len(
self._topic_dict[topic_name]) > 0:
self.type_combo_box.setEditText(self._topic_dict[topic_name][0])
@Slot()
def on_add_publisher_button_clicked(self):
topic_name = str(self.topic_combo_box.currentText())
type_name = str(self.type_combo_box.currentText())
rate = float(self.frequency_combo_box.currentText())
enabled = False
self.add_publisher.emit(topic_name, type_name, rate, enabled)
class PublisherWidget(QWidget):
add_publisher = Signal(str, str, float, bool)
change_publisher = Signal(int, str, str, str, object)
publish_once = Signal(int)
remove_publisher = Signal(int)
clean_up_publishers = Signal()
def __init__(self, parent=None):
super(PublisherWidget, self).__init__(parent)
self._topic_dict = {}
self._update_thread = WorkerThread(self._update_thread_run,
self._update_finished)
self._rospack = rospkg.RosPack()
ui_file = os.path.join(self._rospack.get_path('rqt_publisher'),
'resource', 'Publisher.ui')
loadUi(
ui_file, self, {
'ExtendedComboBox': ExtendedComboBox,
'PublisherTreeWidget': PublisherTreeWidget
})
self.refresh_button.setIcon(QIcon.fromTheme('view-refresh'))
self.refresh_button.clicked.connect(self.refresh_combo_boxes)
self.add_publisher_button.setIcon(QIcon.fromTheme('list-add'))
self.remove_publisher_button.setIcon(QIcon.fromTheme('list-remove'))
self.clear_button.setIcon(QIcon.fromTheme('edit-clear'))
self.refresh_combo_boxes()
self.publisher_tree_widget.model().item_value_changed.connect(
self.change_publisher)
self.publisher_tree_widget.remove_publisher.connect(
self.remove_publisher)
self.publisher_tree_widget.publish_once.connect(self.publish_once)
self.remove_publisher_button.clicked.connect(
self.publisher_tree_widget.remove_selected_publishers)
self.clear_button.clicked.connect(self.clean_up_publishers)
def shutdown_plugin(self):
self._update_thread.kill()
@Slot()
def refresh_combo_boxes(self):
self._update_thread.kill()
self.type_combo_box.setEnabled(False)
self.topic_combo_box.setEnabled(False)
self.type_combo_box.setEditText('updating...')
self.topic_combo_box.setEditText('updating...')
self._update_thread.start()
# this runs in a non-gui thread, so don't access widgets here directly
def _update_thread_run(self):
# update type_combo_box
message_type_names = []
try:
# this only works on fuerte and up
packages = sorted([
pkg_tuple[0] for pkg_tuple in rosmsg.iterate_packages(
self._rospack, rosmsg.MODE_MSG)
])
except:
# this works up to electric
packages = sorted(rosmsg.list_packages())
for package in packages:
for base_type_str in rosmsg.list_msgs(package,
rospack=self._rospack):
message_class = roslib.message.get_message_class(base_type_str)
if message_class is not None:
message_type_names.append(base_type_str)
self.type_combo_box.setItems.emit(sorted(message_type_names))
# update topic_combo_box
_, _, topic_types = rospy.get_master().getTopicTypes()
self._topic_dict = dict(topic_types)
self.topic_combo_box.setItems.emit(sorted(self._topic_dict.keys()))
@Slot()
def _update_finished(self):
self.type_combo_box.setEnabled(True)
self.topic_combo_box.setEnabled(True)
@Slot(str)
def on_topic_combo_box_currentIndexChanged(self, topic_name):
if topic_name in self._topic_dict:
self.type_combo_box.setEditText(self._topic_dict[topic_name])
@Slot()
def on_add_publisher_button_clicked(self):
topic_name = str(self.topic_combo_box.currentText())
type_name = str(self.type_combo_box.currentText())
rate = float(self.frequency_combo_box.currentText())
enabled = False
self.add_publisher.emit(topic_name, type_name, rate, enabled)
class WanderingFrame(QFrame):
def __init__(self, parent=None):
super(WanderingFrame, self).__init__(parent)
self._ui = Ui_wandering_frame()
self.bump_subscriber = rospy.Subscriber('/mobile_base/events/bumper',
BumperEvent,
self.bumper_event_callback)
self._motion = SafeWandering('/mobile_base/commands/velocity', '/odom',
'/mobile_base/events/bumper',
'/mobile_base/events/cliff')
self._motion_thread = None
self._is_alive = False # Used to indicate whether the frame is alive or not (see hibernate/restore methods)
def setupUi(self):
self._ui.setupUi(self)
self._ui.start_button.setEnabled(True)
self._ui.stop_button.setEnabled(False)
self._motion.init(self._ui.speed_spinbox.value(), -0.1,
self._ui.angular_speed_spinbox.value())
def shutdown(self):
'''
Used to terminate the plugin
'''
rospy.loginfo("Kobuki TestSuite: wandering test shutdown")
self._motion.shutdown()
self.bump_subscriber.unregister()
##########################################################################
# Widget Management
##########################################################################
def hibernate(self):
'''
This gets called when the frame goes out of focus (tab switch).
Disable everything to avoid running N tabs in parallel when in
reality we are only running one.
'''
pass
def restore(self):
'''
Restore the frame after a hibernate.
'''
pass
##########################################################################
# Motion Callbacks
##########################################################################
def stop(self):
self._motion.stop()
self._ui.start_button.setEnabled(True)
self._ui.stop_button.setEnabled(False)
def _run_finished(self):
self._ui.start_button.setEnabled(True)
self._ui.stop_button.setEnabled(False)
##########################################################################
# Qt Callbacks
##########################################################################
@Slot()
def on_start_button_clicked(self):
self._ui.start_button.setEnabled(False)
self._ui.stop_button.setEnabled(True)
self._motion_thread = WorkerThread(self._motion.execute,
self._run_finished)
self._motion_thread.start()
@Slot()
def on_stop_button_clicked(self):
self.stop()
@pyqtSlot(float)
def on_speed_spinbox_valueChanged(self, value):
# could use value, but easy to set like this
self._motion.init(self._ui.speed_spinbox.value(), -0.1,
self._ui.angular_speed_spinbox.value())
@pyqtSlot(float)
def on_angular_speed_spinbox_valueChanged(self, value):
# could use value, but easy to set like this
self._motion.init(self._ui.speed_spinbox.value(), -0.1,
self._ui.angular_speed_spinbox.value())
##########################################################################
# Ros Callbacks
##########################################################################
def bumper_event_callback(self, msg):
if msg.state == BumperEvent.PRESSED:
if msg.bumper == BumperEvent.LEFT:
self._ui.left_bump_counter_lcd.display(
self._ui.left_bump_counter_lcd.intValue() + 1)
elif msg.bumper == BumperEvent.RIGHT:
self._ui.right_bump_counter_lcd.display(
self._ui.right_bump_counter_lcd.intValue() + 1)
else:
self._ui.centre_bump_counter_lcd.display(
self._ui.centre_bump_counter_lcd.intValue() + 1)
class Conman(Plugin):
update_graph_sig = Signal(str)
def __init__(self, context):
super(Conman, self).__init__(context)
self._dotcode_sub = None
self._topic_dict = {}
self._update_thread = WorkerThread(self._update_thread_run,
self._update_finished)
# Give QObjects reasonable names
self.setObjectName('Conman')
# 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._widget = QWidget()
# Get path to UI file which is a sibling of this file
# in this example the .ui and .py file are in the same folder
ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)),
'rqt_conman.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.ns_refresh_button.setIcon(QIcon.fromTheme('view-refresh'))
self._widget.setObjectName('ConmanPluginUi')
# 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)
palette = QPalette()
palette.setColor(QPalette.Background, Qt.white)
self._widget.setPalette(palette)
#self._widget.subscribe_button.setCheckable(True)
self._widget.namespace_input.currentIndexChanged.connect(
self._handle_refresh_clicked)
self._widget.ns_refresh_button.clicked.connect(self.refresh_combo_box)
self._widget.refresh_button.clicked[bool].connect(
self._handle_refresh_clicked)
self._widget.commit_button.clicked[bool].connect(
self._handle_commit_clicked)
#self._widget.xdot_widget.connect(
#self._widget.xdot_widget, SIGNAL('_update_graph'), self._widget.xdot_widget.set_dotcode)
self.update_graph_sig.connect(self._update_graph)
self.blocks = {}
self.groups = {}
self._ns = ""
self._actions_connected = False
self.enable_widgets(False)
self.new_dotcode_data = ''
self.update_timer = QTimer(self)
self.update_timer.setInterval(50)
self.update_timer.timeout.connect(self._update_widgets)
#self.update_timer.start()
self._get_blocks = None
self._set_blocks = None
self._blocks_model = QStandardItemModel(0, 4)
self._blocks_model.setHeaderData(0, Qt.Horizontal, "")
self._blocks_model.setHeaderData(1, Qt.Horizontal, "Action")
self._blocks_model.setHeaderData(2, Qt.Horizontal, "State")
self._blocks_model.setHeaderData(3, Qt.Horizontal, "Block")
self._widget.blocks_table.setModel(self._blocks_model)
self._blocks_delegate = BlocksDelegate(self)
self._widget.blocks_table.setItemDelegate(self._blocks_delegate)
self._blocks_model.itemChanged.connect(self.block_changed)
self._groups_model = QStandardItemModel(0, 4)
self._groups_model.setHeaderData(0, Qt.Horizontal, "")
self._groups_model.setHeaderData(1, Qt.Horizontal, "")
self._groups_model.setHeaderData(2, Qt.Horizontal, "")
self._groups_model.setHeaderData(3, Qt.Horizontal, "Group")
self._widget.groups_table.setModel(self._groups_model)
self._groups_delegate = GroupsDelegate(self)
self._widget.groups_table.setItemDelegate(self._groups_delegate)
self.refresh_combo_box()
def block_changed(self, item):
row = item.row()
name = self._blocks_model.item(row, 3).text()
block = self.blocks[name]
checked = item.checkState() == Qt.Checked
def shutdown_plugin(self):
# TODO unregister all publishers here
self._update_thread.kill()
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
# This will enable a setting button (gear icon) in each dock widget title bar
# Usually used to open a modal configuration dialog
@Slot()
def refresh_combo_box(self):
self._update_thread.kill()
self._widget.namespace_input.setEnabled(False)
self._widget.namespace_input.setEditText('updating...')
self._update_thread.start()
def _update_thread_run(self):
_, _, topic_types = rospy.get_master().getTopicTypes()
self._topic_dict = dict(topic_types)
keys = list(self._topic_dict.keys())
namespaces = list()
for i in keys:
if i.endswith("get_blocks_action/goal"):
namespaces.append(i[0:i.index("get_blocks_action/goal")])
self._widget.namespace_input.setItems.emit(namespaces)
@Slot()
def _update_finished(self):
self._widget.namespace_input.setEnabled(True)
def _get_result_cb(self, status, res):
rospy.loginfo("got result!")
self._blocks_model.setRowCount(0)
self._blocks_model.setRowCount(len(res.blocks))
for (i, block) in zip(range(len(res.blocks)), res.blocks):
# Store in dict
self.blocks[block.name] = block
cb = QStandardItem(True)
cb.setCheckable(True)
cb.setCheckState(Qt.Checked if block.state.value == conman_msgs.
msg.TaskState.RUNNING else Qt.Unchecked)
cb.setTextAlignment(Qt.AlignHCenter)
cb.setTristate(True)
action = QStandardItem(True)
action.setText("")
state = QStandardItem(True)
state.setText(state_map[block.state.value])
name = QStandardItem(True)
name.setText(str(block.name))
self._blocks_model.setItem(i, 0, cb)
self._blocks_model.setItem(i, 1, action)
self._blocks_model.setItem(i, 2, state)
self._blocks_model.setItem(i, 3, name)
for (i, group) in zip(range(len(res.groups)), res.groups):
self.groups[group.name] = group
cb = QStandardItem(True)
cb.setCheckable(True)
cb.setCheckState(Qt.Checked)
cb.setTextAlignment(Qt.AlignHCenter)
cb.setEnabled(False)
name = QStandardItem(True)
name.setText(str(group.name))
self._groups_model.setItem(i, 0, cb)
self._groups_model.setItem(i, 3, name)
self._update_groups()
self._update_blocks()
self._widget.blocks_table.resizeColumnsToContents()
self._widget.blocks_table.horizontalHeader().setStretchLastSection(
True)
self._widget.groups_table.resizeColumnsToContents()
self._widget.groups_table.horizontalHeader().setStretchLastSection(
True)
def _update_blocks(self):
for (name, block) in self.blocks.items():
items = self._blocks_model.findItems(name, column=3)
if len(items) > 0:
item = items[0]
else:
continue
row = item.row()
checked = self._blocks_model.item(row,
0).checkState() == Qt.Checked
if checked and block.state.value != conman_msgs.msg.TaskState.RUNNING:
self._blocks_model.item(row, 1).setText("ENABLE")
elif not checked and block.state.value == conman_msgs.msg.TaskState.RUNNING:
self._blocks_model.item(row, 1).setText("DISABLE")
else:
self._blocks_model.item(row, 1).setText("")
self._update_groups()
def _enable_group(self, index, enable):
name = self._groups_model.item(index, 3).text()
group = self.groups[name]
for member in group.members:
items = self._blocks_model.findItems(member, column=3)
if len(items) > 0:
item = items[0]
else:
continue
row = item.row()
self._blocks_model.item(
row, 0).setCheckState(Qt.Checked if enable else Qt.Unchecked)
self._update_blocks()
def _update_groups(self):
for (name, group) in self.groups.items():
group_items = self._groups_model.findItems(name, column=3)
if len(group_items) > 0:
group_item = group_items[0]
else:
continue
group_row = group_item.row()
members_checked = []
for member in group.members:
items = self._blocks_model.findItems(member, column=3)
if len(items) > 0:
item = items[0]
else:
continue
row = item.row()
members_checked.append(
self._blocks_model.item(row, 0).checkState() == Qt.Checked)
if all(members_checked):
check = Qt.Checked
else:
check = Qt.Unchecked
self._groups_model.item(group_row, 0).setCheckState(check)
def _query_blocks(self):
if self._get_blocks and self._actions_connected:
if self._get_blocks.simple_state == actionlib.SimpleGoalState.DONE:
rospy.loginfo("Getting blocks...")
goal = conman_msgs.msg.GetBlocksGoal()
goal.publish_flow_graph = self._widget.generate_graph_checkbox.isChecked(
)
self._get_blocks.send_goal(goal, done_cb=self._get_result_cb)
def enable_widgets(self, enable):
#self._widget.generate_graph_checkbox.setEnabled(enable)
self._widget.force_enable_checkbox.setEnabled(enable)
#self._widget.disable_unused_button.setEnabled(enable)
self._widget.xdot_widget.setEnabled(enable)
self._widget.blocks_table.setEnabled(enable)
self._widget.groups_table.setEnabled(enable)
self._widget.regenerate_graph_button.setEnabled(enable)
def _handle_refresh_clicked(self, checked):
ns = self._widget.namespace_input.currentText()
if len(ns) > 0:
if self._ns != ns:
self._actions_connected = False
self._ns = ns
self.enable_widgets(False)
self._dotcode_sub = rospy.Subscriber(ns + '/dotcode',
std_msgs.msg.String,
self._dotcode_msg_cb)
self._get_blocks = actionlib.SimpleActionClient(
ns + '/get_blocks_action', conman_msgs.msg.GetBlocksAction)
self._set_blocks = actionlib.SimpleActionClient(
ns + '/set_blocks_action', conman_msgs.msg.SetBlocksAction)
rospy.loginfo("Creating action clients on namespace '%s'..." %
ns)
if not self._get_blocks.wait_for_server(rospy.Duration(2)):
rospy.loginfo("Timed out waiting for %s." %
self._get_blocks.action_client.ns)
return
if not self._set_blocks.wait_for_server(rospy.Duration(2)):
rospy.loginfo("Timed out waiting for %s." %
self._set_blocks.action_client.ns)
return
rospy.loginfo("Action clients created.")
self._actions_connected = True
self.enable_widgets(True)
self._query_blocks()
def _handle_commit_clicked(self, checked):
if self._set_blocks and self._actions_connected:
if self._get_blocks.simple_state == actionlib.SimpleGoalState.DONE:
rospy.loginfo("Setting blocks...")
goal = conman_msgs.msg.SetBlocksGoal()
goal.diff = True
goal.force = True
for i in range(self._blocks_model.rowCount()):
name = self._blocks_model.item(i, 3).text()
action = self._blocks_model.item(i, 1).text()
if action == 'DISABLE':
goal.disable.append(name)
elif action == 'ENABLE':
goal.enable.append(name)
self._set_blocks.send_goal(goal,
done_cb=self._get_set_result_cb)
def _get_set_result_cb(self, status, res):
self._query_blocks()
@Slot(str)
def _update_graph(self, dotcode):
global initialized
if initialized:
self._widget.xdot_widget.set_dotcode(dotcode, center=False)
else:
self._widget.xdot_widget.set_dotcode(dotcode, center=True)
self._widget.xdot_widget.zoom_to_fit()
initialized = 1
def _dotcode_msg_cb(self, msg):
#self.new_dotcode_data = msg.data
self.update_graph_sig.emit(msg.data)
def _update_widgets(self):
self._update_groups()
self._update_blocks()
class Smach(Plugin):
update_graph_sig = Signal(str)
def __init__(self, context):
super(Smach, self).__init__(context)
self.initialized = 0
self._dotcode_sub = None
self._topic_dict = {}
self._update_thread = WorkerThread(self._update_thread_run,
self._update_finished)
# Give QObjects reasonable names
self.setObjectName('Smach')
# 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._widget = QWidget()
# Get path to UI file which is a sibling of this file
# in this example the .ui and .py file are in the same folder
ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)),
'rqt_smach.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.ns_refresh_button.setIcon(QIcon.fromTheme('view-refresh'))
self._widget.setObjectName('SmachPluginUi')
# 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)
palette = QPalette()
palette.setColor(QPalette.Background, Qt.white)
self._widget.setPalette(palette)
#Connect widgets with corresponding methods
self._widget.namespace_input.currentIndexChanged.connect(
self._handle_ns_changed)
self._widget.ns_refresh_button.clicked.connect(self.refresh_combo_box)
self._widget.restrict_ns.clicked.connect(self.refresh_combo_box)
self._widget.ud_path_input.currentIndexChanged.connect(
self._handle_ud_path)
self._widget.ud_set_initial.clicked.connect(self._handle_ud_set_path)
self._widget.ud_text_browser.setReadOnly(1)
self._widget.show_implicit_button.clicked.connect(
self._handle_show_implicit)
self._widget.help_button.setIcon(QIcon.fromTheme('help-contents'))
self._widget.help_button.clicked.connect(self._handle_help)
self._widget.tree.clicked.connect(self._handle_tree_clicked)
#Depth and width spinners:
self._widget.depth_input.setRange(-1, 1337)
self._widget.depth_input.setValue(-1)
self._widget.depth_input.valueChanged.connect(self._set_depth)
self._widget.label_width_input.setRange(1, 1337)
self._widget.label_width_input.setValue(40)
self._widget.label_width_input.valueChanged.connect(self._set_width)
self._widget.tree.setColumnCount(1)
self._widget.tree.setHeaderLabels(["Containers"])
self._widget.tree.show()
self._ns = ""
self.refresh_combo_box()
# Bind path list
self._widget.path_input.currentIndexChanged.connect(
self._handle_path_changed)
#Keep Combo Boxes sorted
self._widget.namespace_input.setInsertPolicy(6)
self._widget.path_input.setInsertPolicy(6)
self._widget.ud_path_input.setInsertPolicy(6)
#Set up mouse actions for xdot widget
self._widget.xdot_widget.register_select_callback(self.select_cb)
# Create graph data structures
# Containers is a map of (container path) -> container proxy
self._containers = {}
self._top_containers = {}
# smach introspection client
self._client = smach_ros.IntrospectionClient()
self._selected_paths = []
# Message subscribers
self._structure_subs = {}
self._status_subs = {}
# Initialize xdot display state
self._path = '/'
self._needs_zoom = True
self._structure_changed = True
self._max_depth = -1
self._show_all_transitions = False
self._label_wrapper = textwrap.TextWrapper(40, break_long_words=True)
self._graph_needs_refresh = True
self._tree_needs_refresh = True
self._keep_running = True
self._update_server_list()
self._update_graph()
self._update_tree()
# Start a timer to update the server list
self._server_timer = QTimer(self)
self._server_timer.timeout.connect(self._update_server_list)
self._server_timer.start(1000)
# Start a timer to update the graph display
self._graph_timer = QTimer(self)
self._graph_timer.timeout.connect(self._update_graph)
self._graph_timer.start(1093)
# Start a timer to update the._widget.tree display
self._tree_timer = QTimer(self)
self._tree_timer.timeout.connect(self._update_tree)
self._tree_timer.start(1217)
def _handle_tree_clicked(self):
selected = self._widget.tree.selectedItems()[0]
path = "/" + str(selected.text(0))
parent = selected.parent()
while parent:
path = "/" + str(parent.text(0)) + path
parent = parent.parent()
self._widget.ud_path_input.setCurrentIndex(
self._widget.ud_path_input.findText(path))
def _handle_help(self):
"""Event: Help button pressed"""
helpMsg = QMessageBox()
helpMsg.setWindowTitle("Keyboard Controls")
helpMsg.setIcon(QMessageBox.Information)
helpMsg.setText(
"Pan: Arrow Keys\nZoom: PageUp / PageDown\nZoom To Fit: F\nRefresh: R"
)
ret = helpMsg.exec_()
def select_cb(self, event):
"""Event: Click to select a graph node to display user data and update the graph."""
# Only set string status
x = event.x()
y = event.y()
url = self._widget.xdot_widget.get_url(x, y)
if url is None:
return
item = self._widget.xdot_widget.items_by_url.get(url.url, None)
if item:
self._widget.status_bar.showMessage(item.url)
# Left button-up
if item.url not in self._containers:
if ':' in item.url:
container_url = '/'.join(item.url.split(':')[:-1])
else:
container_url = '/'.join(item.url.split('/')[:-1])
else:
container_url = item.url
if event.button() == Qt.LeftButton:
self._selected_paths = [item.url]
self._widget.ud_path_input.setCurrentIndex(
self._widget.ud_path_input.findText(item.url))
self._graph_needs_refresh = True
def _handle_show_implicit(self):
"""Event: Show Implicit button checked"""
self._show_all_transitions = not self._show_all_transitions
self._graph_needs_refresh = True
self._structure_changed = True
if self._widget.zoom_checkbox.isChecked():
self._needs_zoom = True
def _set_depth(self):
"""Event: Depth value changed"""
self._max_depth = self._widget.depth_input.value()
self._graph_needs_refresh = True
self._structure_changed = True
if self._widget.zoom_checkbox.isChecked():
self._needs_zoom = True
def _set_width(self):
"""Event: Label Width value changed"""
self._label_wrapper.width = self._widget.label_width_input.value()
self._graph_needs_refresh = True
def _handle_ud_set_path(self):
"""Event: Set as Initial State button pressed"""
state_path = self._widget.ud_path_input.currentText()
parent_path = get_parent_path(state_path)
if len(parent_path) > 0:
state = get_label(state_path)
server_name = self._containers[parent_path]._server_name
self._client.set_initial_state(server_name,
parent_path, [state],
timeout=rospy.Duration(60.0))
self._structure_changed = True
self._graph_needs_refresh = True
if self._widget.zoom_checkbox.isChecked():
self._needs_zoom = True
def _handle_ud_path(self):
"""Event: User Data selection combo box changed"""
path = self._widget.ud_path_input.currentText()
#Check the path is non-zero length
if len(path) > 0:
# Get the container corresponding to this path, since userdata is
# stored in the containers
if path not in self._containers:
parent_path = get_parent_path(path)
else:
parent_path = path
if parent_path not in self._containers:
parent_path = get_parent_path(parent_path)
if parent_path in self._containers:
# Get the container
container = self._containers[parent_path]
# Generate the userdata string
ud_str = ''
for (k, v) in container._local_data._data.iteritems():
ud_str += str(k) + ": "
vstr = str(v)
# Add a line break if this is a multiline value
if vstr.find('\n') != -1:
ud_str += '\n'
ud_str += vstr + '\n\n'
#Display the user data
self._widget.ud_text_browser.setPlainText(ud_str)
self._widget.ud_text_browser.show()
def _update_server_list(self):
"""Update the list of known SMACH introspection servers."""
# Discover new servers
if self._widget.restrict_ns.isChecked():
server_names = [self._widget.namespace_input.currentText()[0:-1]]
#self._status_subs = {}
else:
server_names = self._client.get_servers()
new_server_names = [
sn for sn in server_names if sn not in self._status_subs
]
# Create subscribers for newly discovered servers
for server_name in new_server_names:
# Create a subscriber for the plan structure (topology) published by this server
self._structure_subs[server_name] = rospy.Subscriber(
server_name + smach_ros.introspection.STRUCTURE_TOPIC,
SmachContainerStructure,
callback=self._structure_msg_update,
callback_args=server_name,
queue_size=50)
# Create a subscriber for the active states in the plan published by this server
self._status_subs[server_name] = rospy.Subscriber(
server_name + smach_ros.introspection.STATUS_TOPIC,
SmachContainerStatus,
callback=self._status_msg_update,
queue_size=50)
def _structure_msg_update(self, msg, server_name):
"""Update the structure of the SMACH plan (re-generate the dotcode)."""
# Just return if we're shutting down
if not self._keep_running:
return
# Get the node path
path = msg.path
pathsplit = path.split('/')
parent_path = '/'.join(pathsplit[0:-1])
rospy.logdebug("RECEIVED: " + path)
rospy.logdebug("CONTAINERS: " + str(self._containers.keys()))
# Initialize redraw flag
needs_redraw = False
# Determine if we need to update one of the proxies or create a new one
if path in self._containers:
rospy.logdebug("UPDATING: " + path)
# Update the structure of this known container
needs_redraw = self._containers[path].update_structure(msg)
else:
rospy.logdebug("CONSTRUCTING: " + path)
# Create a new container
container = ContainerNode(server_name, msg)
self._containers[path] = container
#Add items to ud path combo box
if self._widget.ud_path_input.findText(path) == -1:
self._widget.ud_path_input.addItem(path)
for item in container._children:
if self._widget.ud_path_input.findText(path + "/" +
item) == -1:
self._widget.ud_path_input.addItem(path + "/" + item)
# Store this as a top container if it has no parent
if parent_path == '':
self._top_containers[path] = container
# Append the path to the appropriate widgets
self._append_new_path(path)
# We need to redraw the graph if this container's parent is being viewed
if path.find(self._widget.path_input.currentText()) == 0:
needs_redraw = True
if needs_redraw:
self._structure_changed = True
if self._widget.zoom_checkbox.isChecked():
self._needs_zoom = True
self._tree_needs_refresh = True
self._graph_needs_refresh = True
def _status_msg_update(self, msg):
"""Process status messages."""
# Check if we're in the process of shutting down
if not self._keep_running:
return
# Get the path to the updating conainer
path = msg.path
rospy.logdebug("STATUS MSG: " + path)
# Check if this is a known container
if path in self._containers:
# Get the container and check if the status update requires regeneration
container = self._containers[path]
if container.update_status(msg):
self._graph_needs_refresh = True
self._tree_needs_refresh = True
def _append_new_path(self, path):
"""Append a new path to the path selection widgets"""
if ((not self._widget.restrict_ns.isChecked())
or ((self._widget.restrict_ns.isChecked()) and
(self._widget.namespace_input.currentText() in path))
or (path == self._widget.namespace_input.currentText()[0:-1])):
self._widget.path_input.addItem(path)
def _update_graph(self):
"""This thread continuously updates the graph when it changes.
The graph gets updated in one of two ways:
1: The structure of the SMACH plans has changed, or the display
settings have been changed. In this case, the dotcode needs to be
regenerated.
2: The status of the SMACH plans has changed. In this case, we only
need to change the styles of the graph.
"""
if self._keep_running and self._graph_needs_refresh and not rospy.is_shutdown(
):
# Get the containers to update
containers_to_update = {}
# Check if the path that's currently being viewed is in the
# list of existing containers
if self._path in self._containers:
# Some non-root path
containers_to_update = {
self._path: self._containers[self._path]
}
elif self._path == '/':
# Root path
containers_to_update = self._top_containers
# Check if we need to re-generate the dotcode (if the structure changed)
# TODO: needs_zoom is a misnomer
if self._structure_changed or self._needs_zoom:
dotstr = "digraph {\n\t"
dotstr += ';'.join([
"compound=true",
"outputmode=nodesfirst",
"labeljust=l",
"nodesep=0.5",
"minlen=2",
"mclimit=5",
"clusterrank=local",
"ranksep=0.75",
# "remincross=true",
# "rank=sink",
"ordering=\"\"",
])
dotstr += ";\n"
# Generate the rest of the graph
# TODO: Only re-generate dotcode for containers that have changed
for path, container in containers_to_update.items():
dotstr += container.get_dotcode(self._selected_paths, [],
0, self._max_depth,
self._containers,
self._show_all_transitions,
self._label_wrapper)
# The given path isn't available
if len(containers_to_update) == 0:
dotstr += '"__empty__" [label="Path not available.", shape="plaintext"]'
dotstr += '\n}\n'
# Set the dotcode to the new dotcode, reset the flags
self.set_dotcode(dotstr, zoom=False)
self._structure_changed = False
self._graph_needs_refresh = False
# Update the styles for the graph if there are any updates
for path, container in containers_to_update.items():
container.set_styles(self._selected_paths, 0, self._max_depth,
self._widget.xdot_widget.items_by_url,
self._widget.xdot_widget.subgraph_shapes,
self._containers)
self._widget.xdot_widget.repaint()
def set_dotcode(self, dotcode, zoom=True):
"""Set the xdot view's dotcode and refresh the display."""
# Set the new dotcode
if self._widget.xdot_widget.set_dotcode(dotcode, False):
# Re-zoom if necessary
if zoom or self._needs_zoom:
self._widget.xdot_widget.zoom_to_fit()
self._needs_zoom = False
def _update_tree(self):
"""Update the tree view."""
if self._keep_running and self._tree_needs_refresh and not rospy.is_shutdown(
):
self._tree_nodes = {}
self._widget.tree.clear()
for path, tc in self._top_containers.iteritems():
if ((not self._widget.restrict_ns.isChecked()) or
((self._widget.restrict_ns.isChecked()) and
(self._widget.namespace_input.currentText() in path)) or
(path
== self._widget.namespace_input.currentText()[0:-1])):
self.add_to_tree(path, None)
self._tree_needs_refresh = False
self._widget.tree.sortItems(0, 0)
def add_to_tree(self, path, parent):
"""Add a path to the tree view."""
if parent is None:
container = QTreeWidgetItem()
container.setText(0, self._containers[path]._label)
self._widget.tree.addTopLevelItem(container)
else:
container = QTreeWidgetItem(parent)
container.setText(0, self._containers[path]._label)
# Add children to_tree
for label in self._containers[path]._children:
child_path = '/'.join([path, label])
if child_path in self._containers.keys():
self.add_to_tree(child_path, container)
else:
child = QTreeWidgetItem(container)
child.setText(0, label)
def append_tree(self, container, parent=None):
"""Append an item to the tree view."""
if not parent:
node = QTreeWidgetItem()
node.setText(0, container._label)
self._widget.tree.addTopLevelItem(node)
for child_label in container._children:
child = QTreeWidgetItem(node)
child.setText(0, child_label)
def _update_thread_run(self):
"""Update the list of namespaces."""
_, _, topic_types = rospy.get_master().getTopicTypes()
self._topic_dict = dict(topic_types)
keys = list(self._topic_dict.keys())
namespaces = list()
for i in keys:
print i
if i.endswith("smach/container_status"):
namespaces.append(i[0:i.index("smach/container_status")])
self._widget.namespace_input.setItems.emit(namespaces)
@Slot()
def _update_finished(self):
"""Enable namespace combo box."""
self._widget.namespace_input.setEnabled(True)
def _handle_ns_changed(self):
"""If namespace selection is changed then reinitialize everything."""
ns = self._widget.namespace_input.currentText()
if len(ns) > 0:
if self._ns != ns:
self._actions_connected = False
self._ns = ns
self.enable_widgets(False)
rospy.loginfo("Creating action clients on namespace '%s'..." %
ns)
rospy.loginfo("Action clients created.")
self._actions_connected = True
self.enable_widgets(True)
self._containers = {}
self._top_containers = {}
self._selected_paths = []
self._structure_subs = {}
self._status_subs = {}
self._needs_zoom = True
self._structure_changed = True
self._max_depth = -1
self._show_all_transitions = False
self._graph_needs_refresh = True
self._tree_needs_refresh = True
#self._widget.namespace_input.clear()
self._widget.path_input.clear()
self._widget.ud_path_input.clear()
self._widget.tree.clear()
@Slot()
def refresh_combo_box(self):
"""Refresh namespace combo box."""
self._update_thread.kill()
self._containers = {}
self._top_containers = {}
self._selected_paths = []
self._structure_subs = {}
self._status_subs = {}
self._structure_changed = True
self._tree_needs_refresh = True
#self._graph_needs_refresh = True
#self._widget.namespace_input.clear()
self._widget.path_input.clear()
self._widget.ud_path_input.clear()
self._widget.tree.clear()
if self._widget.restrict_ns.isChecked():
self._widget.namespace_input.setEnabled(False)
self._widget.namespace_input.setEditText('updating...')
self._widget.ns_refresh_button.setEnabled(True)
self._update_thread.start()
else:
self._widget.namespace_input.setEnabled(False)
self._widget.namespace_input.setEditText('Unrestricted')
self._widget.ns_refresh_button.setEnabled(False)
self._graph_needs_refresh = True
self._tree_needs_refresh = True
self._widget.path_input.addItem('/')
def _handle_path_changed(self, checked):
"""If the path input is changed, update graph."""
self._path = self._widget.path_input.currentText()
self._graph_needs_refresh = True
self._structure_changed = True
if self._widget.zoom_checkbox.isChecked():
self._needs_zoom = True
def enable_widgets(self, enable):
"""Enable all widgets."""
self._widget.xdot_widget.setEnabled(enable)
self._widget.path_input.setEnabled(enable)
self._widget.depth_input.setEnabled(enable)
self._widget.label_width_input.setEnabled(enable)
self._widget.ud_path_input.setEnabled(enable)
self._widget.ud_text_browser.setEnabled(enable)
def on_start_button_clicked(self):
self._ui.start_button.setEnabled(False)
self._ui.stop_button.setEnabled(True)
self._state = CliffSensorFrame.STATE_FORWARD
self._motion_thread = WorkerThread(self._motion.execute, self._run_finished)
self._motion_thread.start()
class LifeFrame(QFrame):
STATE_STOPPED = "stopped"
STATE_RUN = "running"
STATE_IDLE = "idle"
def __init__(self, parent=None):
super(LifeFrame, self).__init__(parent)
self._ui = Ui_life_frame()
self._motion = Rotate('/mobile_base/commands/velocity')
self._motion_thread = None
self._timer = QTimer()
#self._timer.setInterval(60000) #60s
self._timer.setInterval(250) #60s
QObject.connect(self._timer, SIGNAL('timeout()'), self,
SLOT('update_progress_callback()'))
self._state = LifeFrame.STATE_STOPPED
self._is_alive = False # Used to indicate whether the frame is alive or not (see hibernate/restore methods)
def setupUi(self):
self._ui.setupUi(self)
self._ui.start_button.setEnabled(True)
self._ui.stop_button.setEnabled(False)
self._motion.init(self._ui.angular_speed_spinbox.value())
def shutdown(self):
'''
Used to terminate the plugin
'''
rospy.loginfo("Kobuki TestSuite: life frame shutdown")
self._motion.shutdown()
##########################################################################
# Widget Management
##########################################################################
def hibernate(self):
'''
This gets called when the frame goes out of focus (tab switch).
Disable everything to avoid running N tabs in parallel when in
reality we are only running one.
'''
pass
def restore(self):
'''
Restore the frame after a hibernate.
'''
pass
##########################################################################
# Motion Callbacks
##########################################################################
def start(self):
self._state = LifeFrame.STATE_RUN
self._ui.run_progress.reset()
self._ui.idle_progress.reset()
self._motion_thread = WorkerThread(self._motion.execute, None)
self._motion_thread.start()
def stop(self):
self._state = LifeFrame.STATE_STOPPED
self._motion.stop()
if self._motion_thread:
self._motion_thread.wait()
##########################################################################
# Qt Callbacks
##########################################################################
@Slot()
def on_start_button_clicked(self):
self._ui.start_button.setEnabled(False)
self._ui.stop_button.setEnabled(True)
self._timer.start()
self.start()
@Slot()
def on_stop_button_clicked(self):
self.stop()
self._timer.stop()
self._ui.start_button.setEnabled(True)
self._ui.stop_button.setEnabled(False)
@pyqtSlot(float)
def on_angular_speed_spinbox_valueChanged(self, value):
self._motion.init(self._ui.angular_speed_spinbox.value())
##########################################################################
# Timer Callbacks
##########################################################################
@Slot()
def update_progress_callback(self):
if self._state == LifeFrame.STATE_RUN:
new_value = self._ui.run_progress.value() + 1
if new_value == self._ui.run_progress.maximum():
print(" Switching to idle")
self._motion.stop()
self._state = LifeFrame.STATE_IDLE
else:
self._ui.run_progress.setValue(new_value)
if self._state == LifeFrame.STATE_IDLE:
new_value = self._ui.idle_progress.value() + 1
if new_value == self._ui.idle_progress.maximum():
print(" Switching to run")
self.start()
else:
self._ui.idle_progress.setValue(new_value)
class GyroDriftFrame(QFrame):
def __init__(self, parent=None):
super(GyroDriftFrame, self).__init__(parent)
self._ui = Ui_gyro_drift_frame()
self._laser_scan_angle_topic_name = "/laser_scan_angle"
self._gyro_scan_angle_topic_name = "/gyro_scan_angle"
self._error_scan_angle_topic_name = "/error_scan_angle"
self._cmd_vel_topic_name = "/mobile_base/commands/velocity"
self._gyro_topic_name = "/mobile_base/sensors/imu_data"
self._motion = None
self._scan_to_angle = None
self._motion_thread = None
def setupUi(self):
self._ui.setupUi(self)
self._plot_layout = QVBoxLayout(self._ui.scan_angle_group_box)
self._plot_widget = PlotWidget()
self._plot_widget.setWindowTitle("Error")
self._plot_layout.addWidget(self._plot_widget)
self._plot_widget.switch_data_plot_widget(FullSizeDataPlot(self._plot_widget))
self._plot_widget.data_plot.dynamic_range = True
self._plot_widget_live = PlotWidget()
self._plot_widget_live.setWindowTitle("Live Graphs")
self._plot_widget_live.switch_data_plot_widget(MatDataPlot(self._plot_widget_live))
self._plot_layout.addWidget(self._plot_widget_live)
self._ui.start_button.setEnabled(True)
self._ui.stop_button.setEnabled(False)
def shutdown(self):
"""
Used to terminate the plugin
"""
rospy.loginfo("Kobuki TestSuite: gyro drift shutdown")
self._stop()
##########################################################################
# Widget Management
##########################################################################
def _pause_plots(self):
"""
Pause plots, more importantly, pause greedy plot rendering
"""
self._plot_widget.enable_timer(False)
self._plot_widget_live.enable_timer(False)
def hibernate(self):
"""
This gets called when the frame goes out of focus (tab switch).
Disable everything to avoid running N tabs in parallel when in
reality we are only running one.
"""
self._stop()
def restore(self):
"""
Restore the frame after a hibernate.
"""
pass
##########################################################################
# Qt Callbacks
##########################################################################
@Slot()
def on_start_button_clicked(self):
self._ui.start_button.setEnabled(False)
self._ui.stop_button.setEnabled(True)
if not self._scan_to_angle:
self._scan_to_angle = ScanToAngle("/scan", self._laser_scan_angle_topic_name)
if not self._motion:
self._motion = DriftEstimation(
self._laser_scan_angle_topic_name,
self._gyro_scan_angle_topic_name,
self._error_scan_angle_topic_name,
self._cmd_vel_topic_name,
self._gyro_topic_name,
)
self._motion.init(self._ui.angular_speed_spinbox.value())
rospy.sleep(0.5)
self._plot_widget.data_plot.reset()
self._plot_widget._start_time = rospy.get_time()
self._plot_widget.enable_timer(True)
try:
self._plot_widget.remove_topic(self._error_scan_angle_topic_name + "/scan_angle")
self._plot_widget_live.remove_topic(self._laser_scan_angle_topic_name + "/scan_angle")
self._plot_widget_live.remove_topic(self._gyro_scan_angle_topic_name + "/scan_angle")
self._plot_widget_live.remove_topic(self._cmd_vel_topic_name + "/angular/z")
except KeyError:
pass
self._plot_widget_live.add_topic(self._cmd_vel_topic_name + "/angular/z")
self._plot_widget_live.add_topic(self._laser_scan_angle_topic_name + "/scan_angle")
self._plot_widget_live.add_topic(self._gyro_scan_angle_topic_name + "/scan_angle")
self._plot_widget.add_topic(self._error_scan_angle_topic_name + "/scan_angle")
self._motion_thread = WorkerThread(self._motion.execute, None)
self._motion_thread.start()
@Slot()
def on_stop_button_clicked(self):
self._stop()
def _stop(self):
self._pause_plots()
if self._scan_to_angle:
self._scan_to_angle.shutdown()
self._scan_to_angle = None
if self._motion:
self._motion.stop()
if self._motion_thread:
self._motion_thread.wait()
self._motion_thread = None
if self._motion:
self._motion.shutdown()
self._motion = None
self._ui.start_button.setEnabled(True)
self._ui.stop_button.setEnabled(False)
@pyqtSlot(float)
def on_angular_speed_spinbox_valueChanged(self, value):
if self._motion:
self._motion.init(self._ui.angular_speed_spinbox.value())
class Smach(Plugin):
update_graph_sig = Signal(str)
def __init__(self, context):
super(Smach, self).__init__(context)
self.initialized = 0
self._dotcode_sub = None
self._topic_dict = {}
self._update_thread = WorkerThread(self._update_thread_run, self._update_finished)
# Give QObjects reasonable names
self.setObjectName('Smach')
# 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._widget = QWidget()
# Get path to UI file which is a sibling of this file
# in this example the .ui and .py file are in the same folder
ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'rqt_smach.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.ns_refresh_button.setIcon(QIcon.fromTheme('view-refresh'))
self._widget.setObjectName('SmachPluginUi')
# 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)
palette = QPalette ()
palette.setColor(QPalette.Background, Qt.white)
self._widget.setPalette(palette)
#Connect widgets with corresponding methods
self._widget.namespace_input.currentIndexChanged.connect(self._handle_ns_changed)
self._widget.ns_refresh_button.clicked.connect(self.refresh_combo_box)
self._widget.restrict_ns.clicked.connect(self.refresh_combo_box)
self._widget.ud_path_input.currentIndexChanged.connect(self._handle_ud_path)
self._widget.ud_set_initial.clicked.connect(self._handle_ud_set_path)
self._widget.ud_text_browser.setReadOnly(1)
self._widget.show_implicit_button.clicked.connect(self._handle_show_implicit)
self._widget.help_button.setIcon(QIcon.fromTheme('help-contents'))
self._widget.help_button.clicked.connect(self._handle_help)
self._widget.tree.clicked.connect(self._handle_tree_clicked)
#Depth and width spinners:
self._widget.depth_input.setRange(-1, 1337)
self._widget.depth_input.setValue(-1)
self._widget.depth_input.valueChanged.connect(self._set_depth)
self._widget.label_width_input.setRange(1, 1337)
self._widget.label_width_input.setValue(40)
self._widget.label_width_input.valueChanged.connect(self._set_width)
self._widget.tree.setColumnCount(1)
self._widget.tree.setHeaderLabels(["Containers"])
self._widget.tree.show()
self._ns = ""
self.refresh_combo_box()
# Bind path list
self._widget.path_input.currentIndexChanged.connect(
self._handle_path_changed)
#Keep Combo Boxes sorted
self._widget.namespace_input.setInsertPolicy(6)
self._widget.path_input.setInsertPolicy(6)
self._widget.ud_path_input.setInsertPolicy(6)
#Set up mouse actions for xdot widget
self._widget.xdot_widget.register_select_callback(self.select_cb)
# Create graph data structures
# Containers is a map of (container path) -> container proxy
self._containers = {}
self._top_containers = {}
# smach introspection client
self._client = smach_ros.IntrospectionClient()
self._selected_paths = []
# Message subscribers
self._structure_subs = {}
self._status_subs = {}
# Initialize xdot display state
self._path = '/'
self._needs_zoom = True
self._structure_changed = True
self._max_depth = -1
self._show_all_transitions = False
self._label_wrapper = textwrap.TextWrapper(40,break_long_words=True)
self._graph_needs_refresh = True
self._tree_needs_refresh = True
self._keep_running = True
self._update_server_list()
self._update_graph()
self._update_tree()
# Start a timer to update the server list
self._server_timer = QTimer(self)
self._server_timer.timeout.connect(self._update_server_list)
self._server_timer.start(1000)
# Start a timer to update the graph display
self._graph_timer = QTimer(self)
self._graph_timer.timeout.connect(self._update_graph)
self._graph_timer.start(1093)
# Start a timer to update the._widget.tree display
self._tree_timer = QTimer(self)
self._tree_timer.timeout.connect(self._update_tree)
self._tree_timer.start(1217)
def _handle_tree_clicked(self):
selected = self._widget.tree.selectedItems()[0]
path = "/" + str(selected.text(0))
parent = selected.parent()
while parent:
path = "/" + str(parent.text(0)) + path
parent = parent.parent()
self._widget.ud_path_input.setCurrentIndex(self._widget.ud_path_input.findText(path))
def _handle_help(self):
"""Event: Help button pressed"""
helpMsg = QMessageBox()
helpMsg.setWindowTitle("Keyboard Controls")
helpMsg.setIcon(QMessageBox.Information)
helpMsg.setText("Pan: Arrow Keys\nZoom: PageUp / PageDown\nZoom To Fit: F\nRefresh: R")
ret = helpMsg.exec_()
def select_cb(self, event):
"""Event: Click to select a graph node to display user data and update the graph."""
# Only set string status
x = event.x()
y = event.y()
url = self._widget.xdot_widget.get_url(x,y)
if url is None:
return
item = self._widget.xdot_widget.items_by_url.get(url.url, None)
if item:
self._widget.status_bar.showMessage(item.url)
# Left button-up
if item.url not in self._containers:
if ':' in item.url:
container_url = '/'.join(item.url.split(':')[:-1])
else:
container_url = '/'.join(item.url.split('/')[:-1])
else:
container_url = item.url
if event.button() == Qt.LeftButton:
self._selected_paths = [item.url]
self._widget.ud_path_input.setCurrentIndex(self._widget.ud_path_input.findText(item.url))
self._graph_needs_refresh = True
def _handle_show_implicit(self):
"""Event: Show Implicit button checked"""
self._show_all_transitions = not self._show_all_transitions
self._graph_needs_refresh = True
self._structure_changed = True
if self._widget.zoom_checkbox.isChecked():
self._needs_zoom = True
def _set_depth(self):
"""Event: Depth value changed"""
self._max_depth = self._widget.depth_input.value()
self._graph_needs_refresh = True
self._structure_changed = True
if self._widget.zoom_checkbox.isChecked():
self._needs_zoom = True
def _set_width(self):
"""Event: Label Width value changed"""
self._label_wrapper.width = self._widget.label_width_input.value()
self._graph_needs_refresh = True
def _handle_ud_set_path(self):
"""Event: Set as Initial State button pressed"""
state_path = self._widget.ud_path_input.currentText()
parent_path = get_parent_path(state_path)
if len(parent_path) > 0:
state = get_label(state_path)
server_name = self._containers[parent_path]._server_name
self._client.set_initial_state(server_name,parent_path,[state],timeout = rospy.Duration(60.0))
self._structure_changed = True
self._graph_needs_refresh = True
if self._widget.zoom_checkbox.isChecked():
self._needs_zoom = True
def _handle_ud_path(self):
"""Event: User Data selection combo box changed"""
path = self._widget.ud_path_input.currentText()
#Check the path is non-zero length
if len(path) > 0:
# Get the container corresponding to this path, since userdata is
# stored in the containers
if path not in self._containers:
parent_path = get_parent_path(path)
else:
parent_path = path
if parent_path not in self._containers:
parent_path = get_parent_path(parent_path)
if parent_path in self._containers:
# Get the container
container = self._containers[parent_path]
# Generate the userdata string
ud_str = ''
for (k,v) in container._local_data._data.iteritems():
ud_str += str(k)+": "
vstr = str(v)
# Add a line break if this is a multiline value
if vstr.find('\n') != -1:
ud_str += '\n'
ud_str+=vstr+'\n\n'
#Display the user data
self._widget.ud_text_browser.setPlainText(ud_str)
self._widget.ud_text_browser.show()
def _update_server_list(self):
"""Update the list of known SMACH introspection servers."""
# Discover new servers
if self._widget.restrict_ns.isChecked():
server_names = [self._widget.namespace_input.currentText()[0:-1]]
#self._status_subs = {}
else:
server_names = self._client.get_servers()
new_server_names = [sn for sn in server_names if sn not in self._status_subs]
# Create subscribers for newly discovered servers
for server_name in new_server_names:
# Create a subscriber for the plan structure (topology) published by this server
self._structure_subs[server_name] = rospy.Subscriber(
server_name+smach_ros.introspection.STRUCTURE_TOPIC,
SmachContainerStructure,
callback = self._structure_msg_update,
callback_args = server_name,
queue_size=50)
# Create a subscriber for the active states in the plan published by this server
self._status_subs[server_name] = rospy.Subscriber(
server_name+smach_ros.introspection.STATUS_TOPIC,
SmachContainerStatus,
callback = self._status_msg_update,
queue_size=50)
def _structure_msg_update(self, msg, server_name):
"""Update the structure of the SMACH plan (re-generate the dotcode)."""
# Just return if we're shutting down
if not self._keep_running:
return
# Get the node path
path = msg.path
pathsplit = path.split('/')
parent_path = '/'.join(pathsplit[0:-1])
rospy.logdebug("RECEIVED: "+path)
rospy.logdebug("CONTAINERS: "+str(self._containers.keys()))
# Initialize redraw flag
needs_redraw = False
# Determine if we need to update one of the proxies or create a new one
if path in self._containers:
rospy.logdebug("UPDATING: "+path)
# Update the structure of this known container
needs_redraw = self._containers[path].update_structure(msg)
else:
rospy.logdebug("CONSTRUCTING: "+path)
# Create a new container
container = ContainerNode(server_name, msg)
self._containers[path] = container
#Add items to ud path combo box
if self._widget.ud_path_input.findText(path) == -1:
self._widget.ud_path_input.addItem(path)
for item in container._children:
if self._widget.ud_path_input.findText(path+"/"+item) == -1:
self._widget.ud_path_input.addItem(path+"/"+item)
# Store this as a top container if it has no parent
if parent_path == '':
self._top_containers[path] = container
# Append the path to the appropriate widgets
self._append_new_path(path)
# We need to redraw the graph if this container's parent is being viewed
if path.find(self._widget.path_input.currentText()) == 0:
needs_redraw = True
if needs_redraw:
self._structure_changed = True
if self._widget.zoom_checkbox.isChecked():
self._needs_zoom = True
self._tree_needs_refresh = True
self._graph_needs_refresh = True
def _status_msg_update(self, msg):
"""Process status messages."""
# Check if we're in the process of shutting down
if not self._keep_running:
return
# Get the path to the updating conainer
path = msg.path
rospy.logdebug("STATUS MSG: "+path)
# Check if this is a known container
if path in self._containers:
# Get the container and check if the status update requires regeneration
container = self._containers[path]
if container.update_status(msg):
self._graph_needs_refresh = True
self._tree_needs_refresh = True
def _append_new_path(self, path):
"""Append a new path to the path selection widgets"""
if ((not self._widget.restrict_ns.isChecked()) or ((self._widget.restrict_ns.isChecked()) and (self._widget.namespace_input.currentText() in path)) or (path == self._widget.namespace_input.currentText()[0:-1])):
self._widget.path_input.addItem(path)
def _update_graph(self):
"""This thread continuously updates the graph when it changes.
The graph gets updated in one of two ways:
1: The structure of the SMACH plans has changed, or the display
settings have been changed. In this case, the dotcode needs to be
regenerated.
2: The status of the SMACH plans has changed. In this case, we only
need to change the styles of the graph.
"""
if self._keep_running and self._graph_needs_refresh and not rospy.is_shutdown():
# Get the containers to update
containers_to_update = {}
# Check if the path that's currently being viewed is in the
# list of existing containers
if self._path in self._containers:
# Some non-root path
containers_to_update = {self._path:self._containers[self._path]}
elif self._path == '/':
# Root path
containers_to_update = self._top_containers
# Check if we need to re-generate the dotcode (if the structure changed)
# TODO: needs_zoom is a misnomer
if self._structure_changed or self._needs_zoom:
dotstr = "digraph {\n\t"
dotstr += ';'.join([
"compound=true",
"outputmode=nodesfirst",
"labeljust=l",
"nodesep=0.5",
"minlen=2",
"mclimit=5",
"clusterrank=local",
"ranksep=0.75",
# "remincross=true",
# "rank=sink",
"ordering=\"\"",
])
dotstr += ";\n"
# Generate the rest of the graph
# TODO: Only re-generate dotcode for containers that have changed
for path,container in containers_to_update.items():
dotstr += container.get_dotcode(
self._selected_paths,[],
0,self._max_depth,
self._containers,
self._show_all_transitions,
self._label_wrapper)
# The given path isn't available
if len(containers_to_update) == 0:
dotstr += '"__empty__" [label="Path not available.", shape="plaintext"]'
dotstr += '\n}\n'
# Set the dotcode to the new dotcode, reset the flags
self.set_dotcode(dotstr,zoom=False)
self._structure_changed = False
self._graph_needs_refresh = False
# Update the styles for the graph if there are any updates
for path,container in containers_to_update.items():
container.set_styles(
self._selected_paths,
0,self._max_depth,
self._widget.xdot_widget.items_by_url,
self._widget.xdot_widget.subgraph_shapes,
self._containers)
self._widget.xdot_widget.repaint()
def set_dotcode(self, dotcode, zoom=True):
"""Set the xdot view's dotcode and refresh the display."""
# Set the new dotcode
if self._widget.xdot_widget.set_dotcode(dotcode, False):
# Re-zoom if necessary
if zoom or self._needs_zoom:
self._widget.xdot_widget.zoom_to_fit()
self._needs_zoom = False
def _update_tree(self):
"""Update the tree view."""
if self._keep_running and self._tree_needs_refresh and not rospy.is_shutdown():
self._tree_nodes = {}
self._widget.tree.clear()
for path,tc in self._top_containers.iteritems():
if ((not self._widget.restrict_ns.isChecked()) or ((self._widget.restrict_ns.isChecked()) and (self._widget.namespace_input.currentText() in path)) or (path == self._widget.namespace_input.currentText()[0:-1])):
self.add_to_tree(path, None)
self._tree_needs_refresh = False
self._widget.tree.sortItems(0,0)
def add_to_tree(self, path, parent):
"""Add a path to the tree view."""
if parent is None:
container = QTreeWidgetItem()
container.setText(0, self._containers[path]._label)
self._widget.tree.addTopLevelItem(container)
else:
container = QTreeWidgetItem(parent)
container.setText(0, self._containers[path]._label)
# Add children to_tree
for label in self._containers[path]._children:
child_path = '/'.join([path,label])
if child_path in self._containers.keys():
self.add_to_tree(child_path, container)
else:
child = QTreeWidgetItem(container)
child.setText(0, label)
def append_tree(self, container, parent = None):
"""Append an item to the tree view."""
if not parent:
node = QTreeWidgetItem()
node.setText(0, container._label)
self._widget.tree.addTopLevelItem(node)
for child_label in container._children:
child = QTreeWidgetItem(node)
child.setText(0, child_label)
def _update_thread_run(self):
"""Update the list of namespaces."""
_, _, topic_types = rospy.get_master().getTopicTypes()
self._topic_dict = dict(topic_types)
keys = list(self._topic_dict.keys())
namespaces = list()
for i in keys:
print i
if i.endswith("smach/container_status"):
namespaces.append(i[0:i.index("smach/container_status")])
self._widget.namespace_input.setItems.emit(namespaces)
@Slot()
def _update_finished(self):
"""Enable namespace combo box."""
self._widget.namespace_input.setEnabled(True)
def _handle_ns_changed(self):
"""If namespace selection is changed then reinitialize everything."""
ns = self._widget.namespace_input.currentText()
if len(ns) > 0:
if self._ns != ns:
self._actions_connected = False
self._ns = ns
self.enable_widgets(False)
rospy.loginfo("Creating action clients on namespace '%s'..." % ns)
rospy.loginfo("Action clients created.")
self._actions_connected = True
self.enable_widgets(True)
self._containers = {}
self._top_containers = {}
self._selected_paths = []
self._structure_subs = {}
self._status_subs = {}
self._needs_zoom = True
self._structure_changed = True
self._max_depth = -1
self._show_all_transitions = False
self._graph_needs_refresh = True
self._tree_needs_refresh = True
#self._widget.namespace_input.clear()
self._widget.path_input.clear()
self._widget.ud_path_input.clear()
self._widget.tree.clear()
@Slot()
def refresh_combo_box(self):
"""Refresh namespace combo box."""
self._update_thread.kill()
self._containers = {}
self._top_containers = {}
self._selected_paths = []
self._structure_subs = {}
self._status_subs = {}
self._structure_changed = True
self._tree_needs_refresh = True
#self._graph_needs_refresh = True
#self._widget.namespace_input.clear()
self._widget.path_input.clear()
self._widget.ud_path_input.clear()
self._widget.tree.clear()
if self._widget.restrict_ns.isChecked():
self._widget.namespace_input.setEnabled(False)
self._widget.namespace_input.setEditText('updating...')
self._widget.ns_refresh_button.setEnabled(True)
self._update_thread.start()
else:
self._widget.namespace_input.setEnabled(False)
self._widget.namespace_input.setEditText('Unrestricted')
self._widget.ns_refresh_button.setEnabled(False)
self._graph_needs_refresh = True
self._tree_needs_refresh = True
self._widget.path_input.addItem('/')
def _handle_path_changed(self, checked):
"""If the path input is changed, update graph."""
self._path = self._widget.path_input.currentText()
self._graph_needs_refresh = True
self._structure_changed = True
if self._widget.zoom_checkbox.isChecked():
self._needs_zoom = True
def enable_widgets(self, enable):
"""Enable all widgets."""
self._widget.xdot_widget.setEnabled(enable)
self._widget.path_input.setEnabled(enable)
self._widget.depth_input.setEnabled(enable)
self._widget.label_width_input.setEnabled(enable)
self._widget.ud_path_input.setEnabled(enable)
self._widget.ud_text_browser.setEnabled(enable)
class AngularErrorFrame(QFrame):
def __init__(self, parent=None):
super(AngularErrorFrame, self).__init__(parent)
self._cmd_vel_topic_name = '/sleo_velocity_controller/cmd_vel'
self._rospack = rospkg.RosPack()
angular_error_file = os.path.join(
self._rospack.get_path('sleo_testsuite'), 'resource/ui',
'angular_error.ui')
loadUi(angular_error_file, self)
self.start_button.setEnabled(True)
self.stop_button.setEnabled(False)
self._config = None
self._table_config = None
self._motion = None
self._motion_thread = None
def init(self, plot_config, _table_config):
self._config = plot_config
self._table_config = _table_config
self._motion = AngularError(self._config.get_cmd_topic(),
self._config.get_odom_topic())
self._motion.init(self.angular_speed_spinbox.value(),
self.angular_spinbox.value(),
self.tolerance_spinBox.value())
##########################################################################
# Motion Callbacks
##########################################################################
def _run_finished(self):
self.start_button.setEnabled(True)
self.stop_button.setEnabled(False)
def stop(self):
self._motion.stop()
self.start_button.setEnabled(True)
self.stop_button.setEnabled(False)
###########################################################################
# Qt callbacks
###########################################################################
@Slot()
def on_start_button_clicked(self):
self.start_button.setEnabled(False)
self.stop_button.setEnabled(True)
self._motion_thread = WorkerThread(self._motion.execute,
self._run_finished)
self._motion_thread.start()
#rospy.loginfo("current:"+self._config.get_cmd_topic())
self._motion.change_topic(self._config.get_cmd_topic(),
self._config.get_odom_topic())
self._config.run(False, False, True, False)
#self._table_config.table()
@Slot()
def on_stop_button_clicked(self):
self.stop()
@pyqtSlot(float)
def on_angular_speed_spinbox_valueChanged(self, value):
# could use value, but easy to set like this
self._motion.init(self.angular_speed_spinbox.value(),
self.angular_spinbox.value(),
self.tolerance_spinBox.value())
@pyqtSlot(float)
def on_angular_spinbox_valueChanged(self, value):
# could use value, but easy to set like this
self._motion.init(self.angular_speed_spinbox.value(),
self.angular_spinbox.value(),
self.tolerance_spinBox.value())
@pyqtSlot(float)
def on_tolerance_spinBox_valueChanged(self, value):
# could use value, but easy to set like this
self._motion.init(self.angular_speed_spinbox.value(),
self.angular_spinbox.value(),
self.tolerance_spinBox.value())
def shutdown(self):
'''
Used to terminate the plugin
'''
rospy.loginfo("Sleo TestSuite: Angular test shutdown")
self._motion.shutdown()
def start(self):
self._state = LifeFrame.STATE_RUN
self._ui.run_progress.reset()
self._ui.idle_progress.reset()
self._motion_thread = WorkerThread(self._motion.execute, None)
self._motion_thread.start()
def on_start_button_clicked(self):
self._ui.start_button.setEnabled(False)
self._ui.stop_button.setEnabled(True)
self._motion_thread = WorkerThread(self._motion.execute, self._run_finished)
self._motion_thread.start()
class PublisherWidget(QWidget):
add_publisher = Signal(str, str, float, bool)
change_publisher = Signal(int, str, str, str, object)
publish_once = Signal(int)
remove_publisher = Signal(int)
clean_up_publishers = Signal()
def __init__(self, parent=None):
super(PublisherWidget, self).__init__(parent)
self._topic_dict = {}
self._update_thread = WorkerThread(self._update_thread_run, self._update_finished)
self._rospack = rospkg.RosPack()
ui_file = os.path.join(self._rospack.get_path('rqt_publisher'), 'resource', 'Publisher.ui')
loadUi(ui_file, self, {'ExtendedComboBox': ExtendedComboBox, 'PublisherTreeWidget': PublisherTreeWidget})
self.refresh_button.setIcon(QIcon.fromTheme('view-refresh'))
self.refresh_button.clicked.connect(self.refresh_combo_boxes)
self.add_publisher_button.setIcon(QIcon.fromTheme('list-add'))
self.remove_publisher_button.setIcon(QIcon.fromTheme('list-remove'))
self.clear_button.setIcon(QIcon.fromTheme('edit-clear'))
self.refresh_combo_boxes()
self.publisher_tree_widget.model().item_value_changed.connect(self.change_publisher)
self.publisher_tree_widget.remove_publisher.connect(self.remove_publisher)
self.publisher_tree_widget.publish_once.connect(self.publish_once)
self.remove_publisher_button.clicked.connect(self.publisher_tree_widget.remove_selected_publishers)
self.clear_button.clicked.connect(self.clean_up_publishers)
def shutdown_plugin(self):
self._update_thread.kill()
@Slot()
def refresh_combo_boxes(self):
self._update_thread.kill()
self.type_combo_box.setEnabled(False)
self.topic_combo_box.setEnabled(False)
self.type_combo_box.setEditText('updating...')
self.topic_combo_box.setEditText('updating...')
self._update_thread.start()
# this runs in a non-gui thread, so don't access widgets here directly
def _update_thread_run(self):
# update type_combo_box
message_type_names = []
try:
# this only works on fuerte and up
packages = sorted([pkg_tuple[0] for pkg_tuple in rosmsg.iterate_packages(self._rospack, rosmsg.MODE_MSG)])
except:
# this works up to electric
packages = sorted(rosmsg.list_packages())
for package in packages:
for base_type_str in rosmsg.list_msgs(package, rospack=self._rospack):
message_class = roslib.message.get_message_class(base_type_str)
if message_class is not None:
message_type_names.append(base_type_str)
self.type_combo_box.setItems.emit(sorted(message_type_names))
# update topic_combo_box
_, _, topic_types = rospy.get_master().getTopicTypes()
self._topic_dict = dict(topic_types)
self.topic_combo_box.setItems.emit(sorted(self._topic_dict.keys()))
@Slot()
def _update_finished(self):
self.type_combo_box.setEnabled(True)
self.topic_combo_box.setEnabled(True)
@Slot(str)
def on_topic_combo_box_currentIndexChanged(self, topic_name):
if topic_name in self._topic_dict:
self.type_combo_box.setEditText(self._topic_dict[topic_name])
@Slot()
def on_add_publisher_button_clicked(self):
topic_name = str(self.topic_combo_box.currentText())
type_name = str(self.type_combo_box.currentText())
rate = float(self.frequency_combo_box.currentText())
enabled = False
self.add_publisher.emit(topic_name, type_name, rate, enabled)
class WanderingFrame(QFrame):
def __init__(self, parent=None):
super(WanderingFrame, self).__init__(parent)
self._ui = Ui_wandering_frame()
self.bump_subscriber = rospy.Subscriber('/mobile_base/events/bumper', BumperEvent, self.bumper_event_callback)
self._motion = SafeWandering('/mobile_base/commands/velocity','/odom', '/mobile_base/events/bumper', '/mobile_base/events/cliff')
self._motion_thread = None
self._is_alive = False # Used to indicate whether the frame is alive or not (see hibernate/restore methods)
def setupUi(self):
self._ui.setupUi(self)
self._ui.start_button.setEnabled(True)
self._ui.stop_button.setEnabled(False)
self._motion.init(self._ui.speed_spinbox.value(), -0.1, self._ui.angular_speed_spinbox.value())
def shutdown(self):
'''
Used to terminate the plugin
'''
rospy.loginfo("Kobuki TestSuite: wandering test shutdown")
self._motion.shutdown()
self.bump_subscriber.unregister()
##########################################################################
# Widget Management
##########################################################################
def hibernate(self):
'''
This gets called when the frame goes out of focus (tab switch).
Disable everything to avoid running N tabs in parallel when in
reality we are only running one.
'''
pass
def restore(self):
'''
Restore the frame after a hibernate.
'''
pass
##########################################################################
# Motion Callbacks
##########################################################################
def stop(self):
self._motion.stop()
self._ui.start_button.setEnabled(True)
self._ui.stop_button.setEnabled(False)
def _run_finished(self):
self._ui.start_button.setEnabled(True)
self._ui.stop_button.setEnabled(False)
##########################################################################
# Qt Callbacks
##########################################################################
@Slot()
def on_start_button_clicked(self):
self._ui.start_button.setEnabled(False)
self._ui.stop_button.setEnabled(True)
self._motion_thread = WorkerThread(self._motion.execute, self._run_finished)
self._motion_thread.start()
@Slot()
def on_stop_button_clicked(self):
self.stop()
@pyqtSlot(float)
def on_speed_spinbox_valueChanged(self, value):
# could use value, but easy to set like this
self._motion.init(self._ui.speed_spinbox.value(), -0.1, self._ui.angular_speed_spinbox.value())
@pyqtSlot(float)
def on_angular_speed_spinbox_valueChanged(self, value):
# could use value, but easy to set like this
self._motion.init(self._ui.speed_spinbox.value(), -0.1, self._ui.angular_speed_spinbox.value())
##########################################################################
# Ros Callbacks
##########################################################################
def bumper_event_callback(self, msg):
if msg.state == BumperEvent.PRESSED:
if msg.bumper == BumperEvent.LEFT:
self._ui.left_bump_counter_lcd.display(self._ui.left_bump_counter_lcd.intValue()+1)
elif msg.bumper == BumperEvent.RIGHT:
self._ui.right_bump_counter_lcd.display(self._ui.right_bump_counter_lcd.intValue()+1)
else:
self._ui.centre_bump_counter_lcd.display(self._ui.centre_bump_counter_lcd.intValue()+1)
class GyroDriftFrame(QFrame):
def __init__(self, parent=None):
super(GyroDriftFrame, self).__init__(parent)
self._ui = Ui_gyro_drift_frame()
self._laser_scan_angle_topic_name = '/laser_scan_angle'
self._gyro_scan_angle_topic_name = '/gyro_scan_angle'
self._error_scan_angle_topic_name = '/error_scan_angle'
self._cmd_vel_topic_name = '/mobile_base/commands/velocity'
self._gyro_topic_name = '/mobile_base/sensors/imu_data'
self._motion = None
self._scan_to_angle = None
self._motion_thread = None
self._plot_widget = None
self._plot_widget_live = None
def setupUi(self):
self._ui.setupUi(self)
self._plot_layout = QVBoxLayout(self._ui.scan_angle_group_box)
self._ui.start_button.setEnabled(True)
self._ui.stop_button.setEnabled(False)
def shutdown(self):
'''
Used to terminate the plugin
'''
rospy.loginfo("Kobuki TestSuite: gyro drift shutdown")
self._stop()
##########################################################################
# Widget Management
##########################################################################
def _pause_plots(self):
'''
Pause plots, more importantly, pause greedy plot rendering
'''
if self._plot_widget:
self._plot_widget.enable_timer(False)
if self._plot_widget_live:
self._plot_widget_live.enable_timer(False)
def hibernate(self):
'''
This gets called when the frame goes out of focus (tab switch).
Disable everything to avoid running N tabs in parallel when in
reality we are only running one.
'''
self._stop()
self._plot_layout.removeWidget(self._plot_widget)
self._plot_layout.removeWidget(self._plot_widget_live)
self._plot_widget = None
self._plot_widget_live = None
def restore(self):
'''
Restore the frame after a hibernate.
'''
self._plot_widget = PlotWidget()
self._plot_widget.setWindowTitle("Error")
self._plot_layout.addWidget(self._plot_widget)
self._plot_widget.switch_data_plot_widget(DataPlot(self._plot_widget))
self._plot_widget.data_plot.dynamic_range = True
self._plot_widget_live = PlotWidget()
self._plot_widget_live.setWindowTitle("Live Graphs")
self._plot_widget_live.switch_data_plot_widget(DataPlot(self._plot_widget_live))
self._plot_layout.addWidget(self._plot_widget_live)
##########################################################################
# Qt Callbacks
##########################################################################
@Slot()
def on_start_button_clicked(self):
self._ui.start_button.setEnabled(False)
self._ui.stop_button.setEnabled(True)
if not self._scan_to_angle:
self._scan_to_angle = ScanToAngle('/scan',self._laser_scan_angle_topic_name)
if not self._motion:
self._motion = DriftEstimation(self._laser_scan_angle_topic_name, self._gyro_scan_angle_topic_name, self._error_scan_angle_topic_name, self._cmd_vel_topic_name,self._gyro_topic_name)
self._motion.init(self._ui.angular_speed_spinbox.value())
rospy.sleep(0.5)
self._plot_widget._start_time = rospy.get_time()
self._plot_widget.enable_timer(True)
try:
self._plot_widget.remove_topic(self._error_scan_angle_topic_name+'/scan_angle')
self._plot_widget_live.remove_topic(self._laser_scan_angle_topic_name+'/scan_angle')
self._plot_widget_live.remove_topic(self._gyro_scan_angle_topic_name+'/scan_angle')
self._plot_widget_live.remove_topic(self._cmd_vel_topic_name+'/angular/z')
except KeyError:
pass
self._plot_widget_live.add_topic(self._cmd_vel_topic_name+'/angular/z')
self._plot_widget_live.add_topic(self._laser_scan_angle_topic_name+'/scan_angle')
self._plot_widget_live.add_topic(self._gyro_scan_angle_topic_name+'/scan_angle')
self._plot_widget.add_topic(self._error_scan_angle_topic_name+'/scan_angle')
self._motion_thread = WorkerThread(self._motion.execute, None)
self._motion_thread.start()
@Slot()
def on_stop_button_clicked(self):
self._stop()
def _stop(self):
self._pause_plots()
if self._scan_to_angle:
self._scan_to_angle.shutdown()
self._scan_to_angle = None
if self._motion:
self._motion.stop()
if self._motion_thread:
self._motion_thread.wait()
self._motion_thread = None
if self._motion:
self._motion.shutdown()
self._motion = None
self._ui.start_button.setEnabled(True)
self._ui.stop_button.setEnabled(False)
@pyqtSlot(float)
def on_angular_speed_spinbox_valueChanged(self, value):
if self._motion:
self._motion.init(self._ui.angular_speed_spinbox.value())
class RosPackGraph(Plugin):
_deferred_fit_in_view = Signal()
def __init__(self, context):
super(RosPackGraph, self).__init__(context)
self.initialized = False
self._current_dotcode = None
self._update_thread = WorkerThread(self._update_thread_run,
self._update_finished)
self._nodes = {}
self._edges = {}
self._options = {}
self._options_serialized = ''
self.setObjectName('RosPackGraph')
rospack = rospkg.RosPack()
rosstack = rospkg.RosStack()
# factory builds generic dotcode items
self.dotcode_factory = PydotFactory()
# self.dotcode_factory = PygraphvizFactory()
# generator builds rosgraph
self.dotcode_generator = RosPackageGraphDotcodeGenerator(
rospack, rosstack)
# dot_to_qt transforms into Qt elements using dot layout
self.dot_to_qt = DotToQtGenerator()
self._widget = QWidget()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_dep'), 'resource',
'RosPackGraph.ui')
loadUi(ui_file, self._widget,
{'InteractiveGraphicsView': InteractiveGraphicsView})
self._widget.setObjectName('RosPackGraphUi')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
self._scene = QGraphicsScene()
self._scene.setBackgroundBrush(Qt.white)
self._widget.graphics_view.setScene(self._scene)
self._widget.depth_combo_box.insertItem(0, self.tr('infinite'), -1)
self._widget.depth_combo_box.insertItem(1, self.tr('1'), 2)
self._widget.depth_combo_box.insertItem(2, self.tr('2'), 3)
self._widget.depth_combo_box.insertItem(3, self.tr('3'), 4)
self._widget.depth_combo_box.insertItem(4, self.tr('4'), 5)
self._widget.depth_combo_box.currentIndexChanged.connect(
self._refresh_rospackgraph)
self._widget.directions_combo_box.insertItem(0, self.tr('depends'), 0)
self._widget.directions_combo_box.insertItem(1, self.tr('depends_on'),
1)
self._widget.directions_combo_box.insertItem(2, self.tr('both'), 2)
self._widget.directions_combo_box.currentIndexChanged.connect(
self._refresh_rospackgraph)
self._widget.package_type_combo_box.insertItem(0, self.tr('wet & dry'),
3)
self._widget.package_type_combo_box.insertItem(1, self.tr('wet only'),
2)
self._widget.package_type_combo_box.insertItem(2, self.tr('dry only'),
1)
self._widget.package_type_combo_box.currentIndexChanged.connect(
self._refresh_rospackgraph)
completionmodel = StackageCompletionModel(
self._widget.filter_line_edit, rospack, rosstack)
completer = RepeatedWordCompleter(completionmodel, self)
completer.setCompletionMode(QCompleter.PopupCompletion)
completer.setWrapAround(True)
completer.setCaseSensitivity(Qt.CaseInsensitive)
self._widget.filter_line_edit.editingFinished.connect(
self._refresh_rospackgraph)
self._widget.filter_line_edit.setCompleter(completer)
self._widget.filter_line_edit.selectionChanged.connect(
self._clear_filter)
self._widget.with_stacks_check_box.clicked.connect(
self._refresh_rospackgraph)
self._widget.mark_check_box.clicked.connect(self._refresh_rospackgraph)
self._widget.colorize_check_box.clicked.connect(
self._refresh_rospackgraph)
self._widget.hide_transitives_check_box.clicked.connect(
self._refresh_rospackgraph)
self._widget.show_system_check_box.clicked.connect(
self._refresh_rospackgraph)
self._widget.refresh_graph_push_button.setIcon(
QIcon.fromTheme('view-refresh'))
self._widget.refresh_graph_push_button.pressed.connect(
self._update_rospackgraph)
self._widget.highlight_connections_check_box.toggled.connect(
self._refresh_rospackgraph)
self._widget.auto_fit_graph_check_box.toggled.connect(
self._refresh_rospackgraph)
self._widget.fit_in_view_push_button.setIcon(
QIcon.fromTheme('zoom-original'))
self._widget.fit_in_view_push_button.pressed.connect(self._fit_in_view)
self._widget.load_dot_push_button.setIcon(
QIcon.fromTheme('document-open'))
self._widget.load_dot_push_button.pressed.connect(self._load_dot)
self._widget.save_dot_push_button.setIcon(
QIcon.fromTheme('document-save-as'))
self._widget.save_dot_push_button.pressed.connect(self._save_dot)
self._widget.save_as_svg_push_button.setIcon(
QIcon.fromTheme('document-save-as'))
self._widget.save_as_svg_push_button.pressed.connect(self._save_svg)
self._widget.save_as_image_push_button.setIcon(
QIcon.fromTheme('image'))
self._widget.save_as_image_push_button.pressed.connect(
self._save_image)
self._deferred_fit_in_view.connect(self._fit_in_view,
Qt.QueuedConnection)
self._deferred_fit_in_view.emit()
context.add_widget(self._widget)
# If in either of following case, this turnes True
# - 1st filtering key is already input by user
# - filtering key is restored
self._filtering_started = False
def shutdown_plugin(self):
self._update_thread.kill()
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value(
'depth_combo_box_index',
self._widget.depth_combo_box.currentIndex())
instance_settings.set_value(
'directions_combo_box_index',
self._widget.directions_combo_box.currentIndex())
instance_settings.set_value(
'package_type_combo_box',
self._widget.package_type_combo_box.currentIndex())
instance_settings.set_value('filter_line_edit_text',
self._widget.filter_line_edit.text())
instance_settings.set_value(
'with_stacks_state',
self._widget.with_stacks_check_box.isChecked())
instance_settings.set_value(
'hide_transitives_state',
self._widget.hide_transitives_check_box.isChecked())
instance_settings.set_value(
'show_system_state',
self._widget.show_system_check_box.isChecked())
instance_settings.set_value('mark_state',
self._widget.mark_check_box.isChecked())
instance_settings.set_value(
'colorize_state', self._widget.colorize_check_box.isChecked())
instance_settings.set_value(
'auto_fit_graph_check_box_state',
self._widget.auto_fit_graph_check_box.isChecked())
instance_settings.set_value(
'highlight_connections_check_box_state',
self._widget.highlight_connections_check_box.isChecked())
def restore_settings(self, plugin_settings, instance_settings):
_str_filter = instance_settings.value('filter_line_edit_text', '')
if (_str_filter == None or _str_filter == '') and \
not self._filtering_started:
_str_filter = '(Separate pkgs by comma)'
else:
self._filtering_started = True
self._widget.depth_combo_box.setCurrentIndex(
int(instance_settings.value('depth_combo_box_index', 0)))
self._widget.directions_combo_box.setCurrentIndex(
int(instance_settings.value('directions_combo_box_index', 0)))
self._widget.package_type_combo_box.setCurrentIndex(
int(instance_settings.value('package_type_combo_box', 0)))
self._widget.filter_line_edit.setText(_str_filter)
self._widget.with_stacks_check_box.setChecked(
instance_settings.value('with_stacks_state', True) in
[True, 'true'])
self._widget.mark_check_box.setChecked(
instance_settings.value('mark_state', True) in [True, 'true'])
self._widget.colorize_check_box.setChecked(
instance_settings.value('colorize_state', False) in [True, 'true'])
self._widget.hide_transitives_check_box.setChecked(
instance_settings.value('hide_transitives_state', False) in
[True, 'true'])
self._widget.show_system_check_box.setChecked(
instance_settings.value('show_system_state', False) in
[True, 'true'])
self._widget.auto_fit_graph_check_box.setChecked(
instance_settings.value('auto_fit_graph_check_box_state', True) in
[True, 'true'])
self._widget.highlight_connections_check_box.setChecked(
instance_settings.value('highlight_connections_check_box_state',
True) in [True, 'true'])
self.initialized = True
self._refresh_rospackgraph()
def _update_rospackgraph(self):
# re-enable controls customizing fetched ROS graph
self._widget.depth_combo_box.setEnabled(True)
self._widget.directions_combo_box.setEnabled(True)
self._widget.package_type_combo_box.setEnabled(True)
self._widget.filter_line_edit.setEnabled(True)
self._widget.with_stacks_check_box.setEnabled(True)
self._widget.mark_check_box.setEnabled(True)
self._widget.colorize_check_box.setEnabled(True)
self._widget.hide_transitives_check_box.setEnabled(True)
self._widget.show_system_check_box.setEnabled(True)
self._refresh_rospackgraph(force_update=True)
def _update_options(self):
self._options['depth'] = self._widget.depth_combo_box.itemData(
self._widget.depth_combo_box.currentIndex())
self._options[
'directions'] = self._widget.directions_combo_box.itemData(
self._widget.directions_combo_box.currentIndex())
self._options[
'package_types'] = self._widget.package_type_combo_box.itemData(
self._widget.package_type_combo_box.currentIndex())
self._options[
'with_stacks'] = self._widget.with_stacks_check_box.isChecked()
self._options['mark_selected'] = self._widget.mark_check_box.isChecked(
)
self._options[
'hide_transitives'] = self._widget.hide_transitives_check_box.isChecked(
)
self._options[
'show_system'] = self._widget.show_system_check_box.isChecked()
# TODO: Allow different color themes
self._options[
'colortheme'] = True if self._widget.colorize_check_box.isChecked(
) else None
self._options['names'] = self._widget.filter_line_edit.text().split(
',')
if self._options['names'] == [u'None']:
self._options['names'] = []
self._options[
'highlight_level'] = 3 if self._widget.highlight_connections_check_box.isChecked(
) else 1
self._options[
'auto_fit'] = self._widget.auto_fit_graph_check_box.isChecked()
def _refresh_rospackgraph(self, force_update=False):
if not self.initialized:
return
self._update_thread.kill()
self._update_options()
# avoid update if options did not change and force_update is not set
new_options_serialized = pickle.dumps(self._options)
if new_options_serialized == self._options_serialized and not force_update:
return
self._options_serialized = pickle.dumps(self._options)
self._scene.setBackgroundBrush(Qt.lightGray)
self._update_thread.start()
# this runs in a non-gui thread, so don't access widgets here directly
def _update_thread_run(self):
self._update_graph(self._generate_dotcode())
@Slot()
def _update_finished(self):
self._scene.setBackgroundBrush(Qt.white)
self._redraw_graph_scene()
# this runs in a non-gui thread, so don't access widgets here directly
def _generate_dotcode(self):
includes = []
excludes = []
for name in self._options['names']:
if name.strip().startswith('-'):
excludes.append(name.strip()[1:])
else:
includes.append(name.strip())
# orientation = 'LR'
descendants = True
ancestors = True
if self._options['directions'] == 1:
descendants = False
if self._options['directions'] == 0:
ancestors = False
return self.dotcode_generator.generate_dotcode(
dotcode_factory=self.dotcode_factory,
selected_names=includes,
excludes=excludes,
depth=self._options['depth'],
with_stacks=self._options['with_stacks'],
descendants=descendants,
ancestors=ancestors,
mark_selected=self._options['mark_selected'],
colortheme=self._options['colortheme'],
hide_transitives=self._options['hide_transitives'],
show_system=self._options['show_system'],
hide_wet=self._options['package_types'] == 1,
hide_dry=self._options['package_types'] == 2)
# this runs in a non-gui thread, so don't access widgets here directly
def _update_graph(self, dotcode):
self._current_dotcode = dotcode
self._nodes, self._edges = self.dot_to_qt.dotcode_to_qt_items(
self._current_dotcode, self._options['highlight_level'])
def _generate_tool_tip(self, url):
if url is not None and ':' in url:
item_type, item_path = url.split(':', 1)
if item_type == 'node':
tool_tip = 'Node:\n %s' % (item_path)
service_names = rosservice.get_service_list(node=item_path)
if service_names:
tool_tip += '\nServices:'
for service_name in service_names:
try:
service_type = rosservice.get_service_type(
service_name)
tool_tip += '\n %s [%s]' % (service_name,
service_type)
except rosservice.ROSServiceIOException as e:
tool_tip += '\n %s' % (e)
return tool_tip
elif item_type == 'topic':
topic_type, topic_name, _ = rostopic.get_topic_type(item_path)
return 'Topic:\n %s\nType:\n %s' % (topic_name, topic_type)
return url
def _redraw_graph_scene(self):
# remove items in order to not garbage nodes which will be continued to be used
for item in self._scene.items():
self._scene.removeItem(item)
self._scene.clear()
for node_item in self._nodes.values():
self._scene.addItem(node_item)
for edge_items in self._edges.values():
for edge_item in edge_items:
edge_item.add_to_scene(self._scene)
self._scene.setSceneRect(self._scene.itemsBoundingRect())
if self._options['auto_fit']:
self._fit_in_view()
def _load_dot(self, file_name=None):
if file_name is None:
file_name, _ = QFileDialog.getOpenFileName(
self._widget, self.tr('Open graph from file'), None,
self.tr('DOT graph (*.dot)'))
if file_name is None or file_name == '':
return
try:
fh = open(file_name, 'rb')
dotcode = fh.read()
fh.close()
except IOError:
return
# disable controls customizing fetched ROS graph
self._widget.depth_combo_box.setEnabled(False)
self._widget.directions_combo_box.setEnabled(False)
self._widget.package_type_combo_box.setEnabled(False)
self._widget.filter_line_edit.setEnabled(False)
self._widget.with_stacks_check_box.setEnabled(False)
self._widget.mark_check_box.setEnabled(False)
self._widget.colorize_check_box.setEnabled(False)
self._widget.hide_transitives_check_box.setEnabled(False)
self._widget.show_system_check_box.setEnabled(False)
self._update_graph(dotcode)
self._redraw_graph_scene()
@Slot()
def _fit_in_view(self):
self._widget.graphics_view.fitInView(self._scene.itemsBoundingRect(),
Qt.KeepAspectRatio)
def _save_dot(self):
file_name, _ = QFileDialog.getSaveFileName(
self._widget, self.tr('Save as DOT'), 'rospackgraph.dot',
self.tr('DOT graph (*.dot)'))
if file_name is None or file_name == '':
return
handle = QFile(file_name)
if not handle.open(QIODevice.WriteOnly | QIODevice.Text):
return
handle.write(self._current_dotcode)
handle.close()
def _save_svg(self):
file_name, _ = QFileDialog.getSaveFileName(
self._widget, self.tr('Save as SVG'), 'rospackgraph.svg',
self.tr('Scalable Vector Graphic (*.svg)'))
if file_name is None or file_name == '':
return
generator = QSvgGenerator()
generator.setFileName(file_name)
generator.setSize((self._scene.sceneRect().size() * 2.0).toSize())
painter = QPainter(generator)
painter.setRenderHint(QPainter.Antialiasing)
self._scene.render(painter)
painter.end()
def _save_image(self):
file_name, _ = QFileDialog.getSaveFileName(
self._widget, self.tr('Save as image'), 'rospackgraph.png',
self.tr('Image (*.bmp *.jpg *.png *.tiff)'))
if file_name is None or file_name == '':
return
img = QImage((self._scene.sceneRect().size() * 2.0).toSize(),
QImage.Format_ARGB32_Premultiplied)
painter = QPainter(img)
painter.setRenderHint(QPainter.Antialiasing)
self._scene.render(painter)
painter.end()
img.save(file_name)
def _clear_filter(self):
if not self._filtering_started:
self._widget.filter_line_edit.setText('')
self._filtering_started = True
class CliffSensorFrame(QFrame):
STATE_FORWARD = "forward"
STATE_BACKWARD = "backward"
STATE_STOPPED = "stopped"
def __init__(self, parent=None):
super(CliffSensorFrame, self).__init__(parent)
self._ui = Ui_cliff_sensor_frame()
self._motion = TravelForward('/mobile_base/commands/velocity', '/odom',
'/mobile_base/events/cliff')
self._motion_thread = None
self._distance = 1.2
self._state = CliffSensorFrame.STATE_FORWARD
self._is_alive = False # Used to indicate whether the frame is alive or not (see hibernate/restore methods)
def setupUi(self):
self._ui.setupUi(self)
self._ui.start_button.setEnabled(True)
self._ui.stop_button.setEnabled(False)
self._motion.init(self._ui.speed_spinbox.value(), self._distance)
def shutdown(self):
'''
Used to terminate the plugin
'''
rospy.loginfo("Kobuki TestSuite: cliff sensor shutdown")
self._motion.shutdown()
##########################################################################
# Widget Management
##########################################################################
def hibernate(self):
'''
This gets called when the frame goes out of focus (tab switch).
Disable everything to avoid running N tabs in parallel when in
reality we are only running one.
'''
pass
def restore(self):
'''
Restore the frame after a hibernate.
'''
pass
##########################################################################
# Motion Callbacks
##########################################################################
def _run_finished(self):
if self._state == CliffSensorFrame.STATE_STOPPED:
return
elif self._state == CliffSensorFrame.STATE_FORWARD:
self._state = CliffSensorFrame.STATE_BACKWARD
self._motion.init(-self._motion.speed, 0.2)
self._motion_thread = WorkerThread(self._motion.execute,
self._run_finished)
self._motion_thread.start()
else:
self._state = CliffSensorFrame.STATE_FORWARD
self._motion.init(-self._motion.speed, self._distance)
self._motion_thread = WorkerThread(self._motion.execute,
self._run_finished)
self._motion_thread.start()
##########################################################################
# Qt Callbacks
##########################################################################
@Slot()
def on_start_button_clicked(self):
self._ui.start_button.setEnabled(False)
self._ui.stop_button.setEnabled(True)
self._state = CliffSensorFrame.STATE_FORWARD
self._motion_thread = WorkerThread(self._motion.execute,
self._run_finished)
self._motion_thread.start()
@Slot()
def on_stop_button_clicked(self):
self._state = CliffSensorFrame.STATE_STOPPED
self.stop()
def stop(self):
self._motion.stop()
if self._motion_thread:
self._motion_thread.wait()
self._ui.start_button.setEnabled(True)
self._ui.stop_button.setEnabled(False)
@pyqtSlot(float)
def on_speed_spinbox_valueChanged(self, value):
self._motion.init(self._ui.speed_spinbox.value(), self._distance)
def on_start_button_clicked(self):
self._ui.start_button.setEnabled(False)
self._ui.stop_button.setEnabled(True)
self._motion_thread = WorkerThread(self._motion.execute,
self._run_finished)
self._motion_thread.start()
def __init__(self, context):
super(RosPackGraph, self).__init__(context)
self.initialized = False
self._current_dotcode = None
self._update_thread = WorkerThread(self._update_thread_run,
self._update_finished)
self._nodes = {}
self._edges = {}
self._options = {}
self._options_serialized = ''
self.setObjectName('RosPackGraph')
rospack = rospkg.RosPack()
rosstack = rospkg.RosStack()
# factory builds generic dotcode items
self.dotcode_factory = PydotFactory()
# self.dotcode_factory = PygraphvizFactory()
# generator builds rosgraph
self.dotcode_generator = RosPackageGraphDotcodeGenerator(
rospack, rosstack)
# dot_to_qt transforms into Qt elements using dot layout
self.dot_to_qt = DotToQtGenerator()
self._widget = QWidget()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_dep'), 'resource',
'RosPackGraph.ui')
loadUi(ui_file, self._widget,
{'InteractiveGraphicsView': InteractiveGraphicsView})
self._widget.setObjectName('RosPackGraphUi')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
self._scene = QGraphicsScene()
self._scene.setBackgroundBrush(Qt.white)
self._widget.graphics_view.setScene(self._scene)
self._widget.depth_combo_box.insertItem(0, self.tr('infinite'), -1)
self._widget.depth_combo_box.insertItem(1, self.tr('1'), 2)
self._widget.depth_combo_box.insertItem(2, self.tr('2'), 3)
self._widget.depth_combo_box.insertItem(3, self.tr('3'), 4)
self._widget.depth_combo_box.insertItem(4, self.tr('4'), 5)
self._widget.depth_combo_box.currentIndexChanged.connect(
self._refresh_rospackgraph)
self._widget.directions_combo_box.insertItem(0, self.tr('depends'), 0)
self._widget.directions_combo_box.insertItem(1, self.tr('depends_on'),
1)
self._widget.directions_combo_box.insertItem(2, self.tr('both'), 2)
self._widget.directions_combo_box.currentIndexChanged.connect(
self._refresh_rospackgraph)
self._widget.package_type_combo_box.insertItem(0, self.tr('wet & dry'),
3)
self._widget.package_type_combo_box.insertItem(1, self.tr('wet only'),
2)
self._widget.package_type_combo_box.insertItem(2, self.tr('dry only'),
1)
self._widget.package_type_combo_box.currentIndexChanged.connect(
self._refresh_rospackgraph)
completionmodel = StackageCompletionModel(
self._widget.filter_line_edit, rospack, rosstack)
completer = RepeatedWordCompleter(completionmodel, self)
completer.setCompletionMode(QCompleter.PopupCompletion)
completer.setWrapAround(True)
completer.setCaseSensitivity(Qt.CaseInsensitive)
self._widget.filter_line_edit.editingFinished.connect(
self._refresh_rospackgraph)
self._widget.filter_line_edit.setCompleter(completer)
self._widget.filter_line_edit.selectionChanged.connect(
self._clear_filter)
self._widget.with_stacks_check_box.clicked.connect(
self._refresh_rospackgraph)
self._widget.mark_check_box.clicked.connect(self._refresh_rospackgraph)
self._widget.colorize_check_box.clicked.connect(
self._refresh_rospackgraph)
self._widget.hide_transitives_check_box.clicked.connect(
self._refresh_rospackgraph)
self._widget.show_system_check_box.clicked.connect(
self._refresh_rospackgraph)
self._widget.refresh_graph_push_button.setIcon(
QIcon.fromTheme('view-refresh'))
self._widget.refresh_graph_push_button.pressed.connect(
self._update_rospackgraph)
self._widget.highlight_connections_check_box.toggled.connect(
self._refresh_rospackgraph)
self._widget.auto_fit_graph_check_box.toggled.connect(
self._refresh_rospackgraph)
self._widget.fit_in_view_push_button.setIcon(
QIcon.fromTheme('zoom-original'))
self._widget.fit_in_view_push_button.pressed.connect(self._fit_in_view)
self._widget.load_dot_push_button.setIcon(
QIcon.fromTheme('document-open'))
self._widget.load_dot_push_button.pressed.connect(self._load_dot)
self._widget.save_dot_push_button.setIcon(
QIcon.fromTheme('document-save-as'))
self._widget.save_dot_push_button.pressed.connect(self._save_dot)
self._widget.save_as_svg_push_button.setIcon(
QIcon.fromTheme('document-save-as'))
self._widget.save_as_svg_push_button.pressed.connect(self._save_svg)
self._widget.save_as_image_push_button.setIcon(
QIcon.fromTheme('image'))
self._widget.save_as_image_push_button.pressed.connect(
self._save_image)
self._deferred_fit_in_view.connect(self._fit_in_view,
Qt.QueuedConnection)
self._deferred_fit_in_view.emit()
context.add_widget(self._widget)
# If in either of following case, this turnes True
# - 1st filtering key is already input by user
# - filtering key is restored
self._filtering_started = False
def __init__(self, context):
super(Smach, self).__init__(context)
self.initialized = 0
self._dotcode_sub = None
self._topic_dict = {}
self._update_thread = WorkerThread(self._update_thread_run,
self._update_finished)
# Give QObjects reasonable names
self.setObjectName('Smach')
# 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._widget = QWidget()
# Get path to UI file which is a sibling of this file
# in this example the .ui and .py file are in the same folder
ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)),
'rqt_smach.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.ns_refresh_button.setIcon(QIcon.fromTheme('view-refresh'))
self._widget.setObjectName('SmachPluginUi')
# 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)
palette = QPalette()
palette.setColor(QPalette.Background, Qt.white)
self._widget.setPalette(palette)
#Connect widgets with corresponding methods
self._widget.namespace_input.currentIndexChanged.connect(
self._handle_ns_changed)
self._widget.ns_refresh_button.clicked.connect(self.refresh_combo_box)
self._widget.restrict_ns.clicked.connect(self.refresh_combo_box)
self._widget.ud_path_input.currentIndexChanged.connect(
self._handle_ud_path)
self._widget.ud_set_initial.clicked.connect(self._handle_ud_set_path)
self._widget.ud_text_browser.setReadOnly(1)
self._widget.show_implicit_button.clicked.connect(
self._handle_show_implicit)
self._widget.help_button.setIcon(QIcon.fromTheme('help-contents'))
self._widget.help_button.clicked.connect(self._handle_help)
self._widget.tree.clicked.connect(self._handle_tree_clicked)
#Depth and width spinners:
self._widget.depth_input.setRange(-1, 1337)
self._widget.depth_input.setValue(-1)
self._widget.depth_input.valueChanged.connect(self._set_depth)
self._widget.label_width_input.setRange(1, 1337)
self._widget.label_width_input.setValue(40)
self._widget.label_width_input.valueChanged.connect(self._set_width)
self._widget.tree.setColumnCount(1)
self._widget.tree.setHeaderLabels(["Containers"])
self._widget.tree.show()
self._ns = ""
self.refresh_combo_box()
# Bind path list
self._widget.path_input.currentIndexChanged.connect(
self._handle_path_changed)
#Keep Combo Boxes sorted
self._widget.namespace_input.setInsertPolicy(6)
self._widget.path_input.setInsertPolicy(6)
self._widget.ud_path_input.setInsertPolicy(6)
#Set up mouse actions for xdot widget
self._widget.xdot_widget.register_select_callback(self.select_cb)
# Create graph data structures
# Containers is a map of (container path) -> container proxy
self._containers = {}
self._top_containers = {}
# smach introspection client
self._client = smach_ros.IntrospectionClient()
self._selected_paths = []
# Message subscribers
self._structure_subs = {}
self._status_subs = {}
# Initialize xdot display state
self._path = '/'
self._needs_zoom = True
self._structure_changed = True
self._max_depth = -1
self._show_all_transitions = False
self._label_wrapper = textwrap.TextWrapper(40, break_long_words=True)
self._graph_needs_refresh = True
self._tree_needs_refresh = True
self._keep_running = True
self._update_server_list()
self._update_graph()
self._update_tree()
# Start a timer to update the server list
self._server_timer = QTimer(self)
self._server_timer.timeout.connect(self._update_server_list)
self._server_timer.start(1000)
# Start a timer to update the graph display
self._graph_timer = QTimer(self)
self._graph_timer.timeout.connect(self._update_graph)
self._graph_timer.start(1093)
# Start a timer to update the._widget.tree display
self._tree_timer = QTimer(self)
self._tree_timer.timeout.connect(self._update_tree)
self._tree_timer.start(1217)
class LifeFrame(QFrame):
STATE_STOPPED = "stopped"
STATE_RUN = "running"
STATE_IDLE = "idle"
def __init__(self, parent=None):
super(LifeFrame, self).__init__(parent)
self._ui = Ui_life_frame()
self._motion = Rotate('/mobile_base/commands/velocity')
self._motion_thread = None
self._timer = QTimer()
#self._timer.setInterval(60000) #60s
self._timer.setInterval(250) #60s
QObject.connect(self._timer, SIGNAL('timeout()'), self, SLOT('update_progress_callback()'))
self._state = LifeFrame.STATE_STOPPED
self._is_alive = False # Used to indicate whether the frame is alive or not (see hibernate/restore methods)
def setupUi(self):
self._ui.setupUi(self)
self._ui.start_button.setEnabled(True)
self._ui.stop_button.setEnabled(False)
self._motion.init(self._ui.angular_speed_spinbox.value())
def shutdown(self):
'''
Used to terminate the plugin
'''
rospy.loginfo("Kobuki TestSuite: life frame shutdown")
self._motion.shutdown()
##########################################################################
# Widget Management
##########################################################################
def hibernate(self):
'''
This gets called when the frame goes out of focus (tab switch).
Disable everything to avoid running N tabs in parallel when in
reality we are only running one.
'''
pass
def restore(self):
'''
Restore the frame after a hibernate.
'''
pass
##########################################################################
# Motion Callbacks
##########################################################################
def start(self):
self._state = LifeFrame.STATE_RUN
self._ui.run_progress.reset()
self._ui.idle_progress.reset()
self._motion_thread = WorkerThread(self._motion.execute, None)
self._motion_thread.start()
def stop(self):
self._state = LifeFrame.STATE_STOPPED
self._motion.stop()
if self._motion_thread:
self._motion_thread.wait()
##########################################################################
# Qt Callbacks
##########################################################################
@Slot()
def on_start_button_clicked(self):
self._ui.start_button.setEnabled(False)
self._ui.stop_button.setEnabled(True)
self._timer.start()
self.start()
@Slot()
def on_stop_button_clicked(self):
self.stop()
self._timer.stop()
self._ui.start_button.setEnabled(True)
self._ui.stop_button.setEnabled(False)
@pyqtSlot(float)
def on_angular_speed_spinbox_valueChanged(self, value):
self._motion.init(self._ui.angular_speed_spinbox.value())
##########################################################################
# Timer Callbacks
##########################################################################
@Slot()
def update_progress_callback(self):
if self._state == LifeFrame.STATE_RUN:
new_value = self._ui.run_progress.value()+1
if new_value == self._ui.run_progress.maximum():
print(" Switching to idle")
self._motion.stop()
self._state = LifeFrame.STATE_IDLE
else:
self._ui.run_progress.setValue(new_value)
if self._state == LifeFrame.STATE_IDLE:
new_value = self._ui.idle_progress.value()+1
if new_value == self._ui.idle_progress.maximum():
print(" Switching to run")
self.start()
else:
self._ui.idle_progress.setValue(new_value)
def start(self):
self._state = LifeFrame.STATE_RUN
self._ui.run_progress.reset()
self._ui.idle_progress.reset()
self._motion_thread = WorkerThread(self._motion.execute, None)
self._motion_thread.start()
class MsgEditorWidget(QWidget):
msg_type_changed = Signal(str)
change_message = Signal(int, str, str, str, object)
clean = Signal()
accept = Signal()
cancel = Signal()
def __init__(self, parent=None):
super(MsgEditorWidget, self).__init__(parent)
self._topic_dict = {}
self._update_thread = WorkerThread(self._update_thread_run, self._update_finished)
self._rospack = rospkg.RosPack()
ui_file = os.path.join(self._rospack.get_path('rqt_annotation_data'), 'resource', 'MsgEditor.ui')
loadUi(ui_file, self, {'ExtendedComboBox': ExtendedComboBox, 'MessageTreeWidget': MessageTreeWidget})
self.refresh_combo_boxes()
self.message_tree_widget.model().item_value_changed.connect(self.change_message)
self.clear_button.clicked.connect(self.clean)
def shutdown_plugin(self):
self._update_thread.kill()
@Slot()
def refresh_combo_boxes(self):
self._update_thread.kill()
self.msg_type_combo_box.setEnabled(False)
self.msg_type_combo_box.setEditText('updating...')
self._update_thread.start()
# this runs in a non-gui thread, so don't access widgets here directly
def _update_thread_run(self):
# update msg_type_combo_box
message_type_names = []
try:
# this only works on fuerte and up
packages = sorted([pkg_tuple[0] for pkg_tuple in rosmsg.iterate_packages(self._rospack, rosmsg.MODE_MSG)])
except:
# this works up to electric
packages = sorted(rosmsg.list_packages())
for package in packages:
for base_type_str in rosmsg.list_msgs(package, rospack=self._rospack):
message_class = roslib.message.get_message_class(base_type_str)
if message_class is not None:
message_type_names.append(base_type_str)
if hasattr(self, 'prev_type_name'):
del self.prev_type_name
message_type_names.append('') # null message type at first
self.msg_type_combo_box.setItems.emit(sorted(message_type_names))
# update topic_combo_box
_, _, topic_types = rospy.get_master().getTopicTypes()
self._topic_dict = dict(topic_types)
@Slot()
def _update_finished(self):
self.msg_type_combo_box.setEnabled(True)
@Slot(str)
def on_msg_type_combo_box_currentIndexChanged(self, type_name):
self.msg_type_changed.emit(type_name)
self.prev_type_name = str(self.msg_type_combo_box.currentText())
@Slot()
def on_accept_button_clicked(self):
self.accept.emit()
def on_cancel_button_clicked(self):
self.cancel.emit()
