def _update_client_tab(self):
print('[conductor graph]: _update_client_tab')
self.pre_selected_client_name = self.cur_selected_client_name
self._widget.tabWidget.clear()
for k in self._graph.concert_clients.values():
# Only pull in information from connected or connectable clients
if k.state not in [
concert_msgs.ConcertClientState.AVAILABLE,
concert_msgs.ConcertClientState.MISSING,
concert_msgs.ConcertClientState.UNINVITED
]:
continue
main_widget = QWidget()
ver_layout = QVBoxLayout(main_widget)
ver_layout.setContentsMargins(9, 9, 9, 9)
ver_layout.setSizeConstraint(ver_layout.SetDefaultConstraint)
#button layout
sub_widget = QWidget()
sub_widget.setAccessibleName('sub_widget')
ver_layout.addWidget(sub_widget)
#client information layout
context_label = QLabel()
context_label.setText("Client information")
ver_layout.addWidget(context_label)
app_context_widget = QPlainTextEdit()
app_context_widget.setObjectName(k.concert_alias + '_' +
'app_context_widget')
app_context_widget.setAccessibleName('app_context_widget')
app_context_widget.appendHtml(k.get_rapp_context())
app_context_widget.setReadOnly(True)
cursor = app_context_widget.textCursor()
cursor.movePosition(QTextCursor.Start, QTextCursor.MoveAnchor, 0)
app_context_widget.setTextCursor(cursor)
ver_layout.addWidget(app_context_widget)
# new icon
path = ""
if k.is_new:
# This only changes when the concert client changes topic publishes anew
path = os.path.join(os.path.dirname(os.path.abspath(__file__)),
"../../resources/images/new.gif")
#add tab
self._widget.tabWidget.addTab(main_widget, QIcon(path),
k.concert_alias)
#set previous selected tab
for k in range(self._widget.tabWidget.count()):
tab_text = self._widget.tabWidget.tabText(k)
if tab_text == self.pre_selected_client_name:
self._widget.tabWidget.setCurrentIndex(k)
class PingGUI(Plugin):
def __init__(self, context):
super(PingGUI, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('PingGUI')
self.msg = None
# Create a container widget and give it a layout
self._container = QWidget()
self._layout = QVBoxLayout()
self._container.setLayout(self._layout)
self._label = QLabel("xx ms latency")
p = self._label.palette()
p.setColor(self._label.backgroundRole(), Qt.red)
self._label.setPalette(p)
self._layout.addWidget(self._label)
self.set_bg_color(100, 100, 100)
rospy.Subscriber("/ping/delay", Float64, self.ping_cb)
context.add_widget(self._container)
self._update_plot_timer = QTimer(self)
self._update_plot_timer.timeout.connect(self.update_gui)
self._update_plot_timer.start(1)
def update_gui(self):
if not self.msg:
return
msg = self.msg
# msec
# 100 -> green, 1000 -> red
# normalize within 1000 ~ 100
orig_latency = msg.data
msg_data = orig_latency
if msg.data > 1000:
msg_data = 1000
elif msg.data < 100:
msg_data = 100
ratio = (msg_data - 100) / (1000 - 100)
color_r = ratio * 255.0
color_g = (1 - ratio) * 255.0
# devnull = open(os.devnull, "w")
# with RedirectStdStreams(stdout=devnull, stderr=devnull):
self.set_bg_color(color_r, color_g, 0)
self._label.setText("%d ms latency" % (orig_latency))
def set_bg_color(self, r, g, b):
self._label.setStyleSheet("QLabel { display: block; background-color: rgba(%d, %d, %d, 255); text-align: center; font-size: 30px;}" % (r, g, b))
def ping_cb(self, msg):
self.msg = msg
def shutdown_plugin(self):
pass
def save_settings(self, plugin_settings, instance_settings):
pass
def restore_settings(self, plugin_settings, instance_settings):
pass
def __init__(self, main, fileName, main_hbox):
self.main = main
self.chain = []
self.poses = []
self.chain_name = ""
frame = QFrame()
frame.setFrameShape(QFrame.StyledPanel);
frame.setFrameShadow(QFrame.Raised);
vbox = QVBoxLayout()
label = QLabel()
vbox.addWidget(label);
self.load_file(fileName, vbox)
self.trajectoryPublisher = rospy.Publisher('/trajectory_controllers/'+self.chain_name+'_traj_controller/trajectory' ,JointTrajectory, queue_size=10)
self.positionPublisher = rospy.Publisher('/trajectory_controllers/'+self.chain_name+'_traj_controller/joint_states',JointState, queue_size=10)
self.ghostPublisher = rospy.Publisher('/flor/ghost/set_joint_states',JointState, queue_size=10)
label.setText(self.chain_name)
vbox.addStretch(1)
frame.setLayout(vbox)
main_hbox.addWidget(frame)
def __init__(self, parent, fileName, top_widget_layout):
self.controllers = []
self.parent = parent
self.loadFile(fileName)
print "Initialize controllers..."
for controller in self.controllers:
frame = QFrame()
frame.setFrameShape(QFrame.StyledPanel);
frame.setFrameShadow(QFrame.Raised);
vbox = QVBoxLayout()
label = QLabel()
label.setText(controller.label)
vbox.addWidget(label);
print controller.name
for joint in controller.joints:
label = QLabel()
label.setText(joint.name)
vbox.addWidget(label);
#Add input for setting the biases
widget = QWidget()
hbox = QHBoxLayout()
hbox.addWidget(joint.sensor_bias_spinbox)
hbox.addWidget(joint.control_bias_spinbox)
hbox.addWidget(joint.gearing_bias_spinbox)
widget.setLayout(hbox)
vbox.addWidget(widget)
label = QLabel()
label.setText(" Sensor Control Gearing")
vbox.addWidget(label);
vbox.addStretch()
frame.setLayout(vbox)
top_widget_layout.addWidget(frame)
print "Done loading controllers"
class GroupWidget(QWidget):
'''
(Isaac's guess as of 12/13/2012)
This class bonds multiple Editor instances that are associated with
a single node as a group.
'''
# public signal
sig_node_disabled_selected = Signal(str)
def __init__(self, updater, config, nodename):
'''
:param config:
:type config: Dictionary? defined in dynamic_reconfigure.client.Client
:type nodename: str
'''
#TODO figure out what data type 'config' is. It is afterall returned
# from dynamic_reconfigure.client.get_parameter_descriptions()
# ros.org/doc/api/dynamic_reconfigure/html/dynamic_reconfigure.client-pysrc.html#Client
super(GroupWidget, self).__init__()
self.state = config['state']
self.name = config['name']
self._toplevel_treenode_name = nodename
# TODO: .ui file needs to be back into usage in later phase.
# ui_file = os.path.join(rp.get_path('rqt_reconfigure'),
# 'resource', 'singlenode_parameditor.ui')
# loadUi(ui_file, self)
verticalLayout = QVBoxLayout(self)
verticalLayout.setContentsMargins(QMargins(0, 0, 0, 0))
_widget_nodeheader = QWidget()
_h_layout_nodeheader = QHBoxLayout(_widget_nodeheader)
_h_layout_nodeheader.setContentsMargins(QMargins(0, 0, 0, 0))
self.nodename_qlabel = QLabel(self)
font = QFont('Trebuchet MS, Bold')
font.setUnderline(True)
font.setBold(True)
# Button to close a node.
_icon_disable_node = QIcon.fromTheme('window-close')
_bt_disable_node = QPushButton(_icon_disable_node, '', self)
_bt_disable_node.setToolTip('Hide this node')
_bt_disable_node_size = QSize(36, 24)
_bt_disable_node.setFixedSize(_bt_disable_node_size)
_bt_disable_node.pressed.connect(self._node_disable_bt_clicked)
_h_layout_nodeheader.addWidget(self.nodename_qlabel)
_h_layout_nodeheader.addWidget(_bt_disable_node)
self.nodename_qlabel.setAlignment(Qt.AlignCenter)
font.setPointSize(10)
self.nodename_qlabel.setFont(font)
grid_widget = QWidget(self)
self.grid = QFormLayout(grid_widget)
verticalLayout.addWidget(_widget_nodeheader)
verticalLayout.addWidget(grid_widget, 1)
# Again, these UI operation above needs to happen in .ui file.
self.tab_bar = None # Every group can have one tab bar
self.tab_bar_shown = False
self.updater = updater
self.editor_widgets = []
self._param_names = []
self._create_node_widgets(config)
rospy.logdebug('Groups node name={}'.format(nodename))
self.nodename_qlabel.setText(nodename)
# Labels should not stretch
#self.grid.setColumnStretch(1, 1)
#self.setLayout(self.grid)
def collect_paramnames(self, config):
pass
def _create_node_widgets(self, config):
'''
:type config: Dict?
'''
i_debug = 0
for param in config['parameters']:
begin = time.time() * 1000
editor_type = '(none)'
if param['edit_method']:
widget = EnumEditor(self.updater, param)
elif param['type'] in EDITOR_TYPES:
rospy.logdebug('GroupWidget i_debug=%d param type =%s',
i_debug,
param['type'])
editor_type = EDITOR_TYPES[param['type']]
widget = eval(editor_type)(self.updater, param)
self.editor_widgets.append(widget)
self._param_names.append(param['name'])
rospy.logdebug('groups._create_node_widgets num editors=%d',
i_debug)
end = time.time() * 1000
time_elap = end - begin
rospy.logdebug('ParamG editor={} loop=#{} Time={}msec'.format(
editor_type, i_debug, time_elap))
i_debug += 1
for name, group in config['groups'].items():
if group['type'] == 'tab':
widget = TabGroup(self, self.updater, group)
elif group['type'] in _GROUP_TYPES.keys():
widget = eval(_GROUP_TYPES[group['type']])(self.updater, group)
self.editor_widgets.append(widget)
rospy.logdebug('groups._create_node_widgets ' +
#'num groups=%d' +
'name=%s',
name)
for i, ed in enumerate(self.editor_widgets):
ed.display(self.grid)
rospy.logdebug('GroupWdgt._create_node_widgets len(editor_widgets)=%d',
len(self.editor_widgets))
def display(self, grid, row):
# groups span across all columns
grid.addWidget(self, row, 0, 1, -1)
def update_group(self, config):
self.state = config['state']
# TODO: should use config.keys but this method doesnt exist
names = [name for name in config.items()]
for widget in self.editor_widgets:
if isinstance(widget, EditorWidget):
if widget.name in names:
widget.update_value(config[widget.name])
elif isinstance(widget, GroupWidget):
cfg = find_cfg(config, widget.name)
widget.update_group(cfg)
def close(self):
for w in self.editor_widgets:
w.close()
def get_treenode_names(self):
'''
:rtype: str[]
'''
return self._param_names
def _node_disable_bt_clicked(self):
rospy.logdebug('param_gs _node_disable_bt_clicked')
self.sig_node_disabled_selected.emit(self._toplevel_treenode_name)
class CalibrationMovementsGUI(QWidget):
NOT_INITED_YET = 0
BAD_PLAN = 1
GOOD_PLAN = 2
MOVED_TO_POSE = 3
BAD_STARTING_POSITION = 4
GOOD_STARTING_POSITION = 5
CHECKING_STARTING_POSITION = 6
MOVEMENT_FAILED = 7
def __init__(self):
super(CalibrationMovementsGUI, self).__init__()
self.handeye_client = HandeyeClient()
self.current_target_pose = -1 # -1 is home
self.target_poses = None
self.plan_was_successful = None
self.state = CalibrationMovementsGUI.NOT_INITED_YET
self.layout = QVBoxLayout()
self.labels_layout = QHBoxLayout()
self.buttons_layout = QHBoxLayout()
self.progress_bar = QProgressBar()
self.pose_number_lbl = QLabel('0/0')
self.bad_plan_lbl = QLabel('No plan yet')
self.bad_plan_lbl.setAlignment(Qt.AlignCenter)
self.guide_lbl = QLabel('Hello')
self.guide_lbl.setWordWrap(True)
self.check_start_pose_btn = QPushButton('Check starting pose')
self.check_start_pose_btn.clicked.connect(
self.handle_check_current_state)
self.next_pose_btn = QPushButton('Next Pose')
self.next_pose_btn.clicked.connect(self.handle_next_pose)
self.plan_btn = QPushButton('Plan')
self.plan_btn.clicked.connect(self.handle_plan)
self.execute_btn = QPushButton('Execute')
self.execute_btn.clicked.connect(self.handle_execute)
self.labels_layout.addWidget(self.pose_number_lbl)
self.labels_layout.addWidget(self.bad_plan_lbl)
self.buttons_layout.addWidget(self.check_start_pose_btn)
self.buttons_layout.addWidget(self.next_pose_btn)
self.buttons_layout.addWidget(self.plan_btn)
self.buttons_layout.addWidget(self.execute_btn)
self.layout.addWidget(self.progress_bar)
self.layout.addLayout(self.labels_layout)
self.layout.addWidget(self.guide_lbl)
self.layout.addLayout(self.buttons_layout)
self.setLayout(self.layout)
self.plan_btn.setEnabled(False)
self.execute_btn.setEnabled(False)
self.setWindowTitle('Local Mover')
self.show()
def update_ui(self):
if self.target_poses:
count_target_poses = len(self.target_poses)
else:
count_target_poses = 1
self.progress_bar.setMaximum(count_target_poses)
self.progress_bar.setValue(self.current_target_pose + 1)
self.pose_number_lbl.setText('{}/{}'.format(
self.current_target_pose + 1, count_target_poses))
if self.state == CalibrationMovementsGUI.BAD_PLAN:
self.bad_plan_lbl.setText('BAD plan!! Don\'t do it!!!!')
self.bad_plan_lbl.setStyleSheet('QLabel { background-color : red}')
elif self.state == CalibrationMovementsGUI.GOOD_PLAN:
self.bad_plan_lbl.setText('Good plan')
self.bad_plan_lbl.setStyleSheet(
'QLabel { background-color : green}')
else:
self.bad_plan_lbl.setText('No plan yet')
self.bad_plan_lbl.setStyleSheet('')
if self.state == CalibrationMovementsGUI.NOT_INITED_YET:
self.guide_lbl.setText(
'Bring the robot to a plausible position and check if it is a suitable starting pose'
)
elif self.state == CalibrationMovementsGUI.CHECKING_STARTING_POSITION:
self.guide_lbl.setText(
'Checking if the robot can translate and rotate in all directions from the current pose'
)
elif self.state == CalibrationMovementsGUI.BAD_STARTING_POSITION:
self.guide_lbl.setText('Cannot calibrate from current position')
elif self.state == CalibrationMovementsGUI.GOOD_STARTING_POSITION:
self.guide_lbl.setText('Ready to start: click on next pose')
elif self.state == CalibrationMovementsGUI.GOOD_PLAN:
self.guide_lbl.setText(
'The plan seems good: press execute to move the robot')
elif self.state == CalibrationMovementsGUI.BAD_PLAN:
self.guide_lbl.setText('Planning failed: try again')
elif self.state == CalibrationMovementsGUI.MOVED_TO_POSE:
self.guide_lbl.setText(
'Pose reached: take a sample and go on to next pose')
can_plan = self.state == CalibrationMovementsGUI.GOOD_STARTING_POSITION
self.plan_btn.setEnabled(can_plan)
can_move = self.state == CalibrationMovementsGUI.GOOD_PLAN
self.execute_btn.setEnabled(can_move)
QCoreApplication.processEvents()
def handle_check_current_state(self):
self.state = CalibrationMovementsGUI.CHECKING_STARTING_POSITION
self.update_ui()
res = self.handeye_client.check_starting_pose()
if res.can_calibrate:
self.state = CalibrationMovementsGUI.GOOD_STARTING_POSITION
else:
self.state = CalibrationMovementsGUI.BAD_STARTING_POSITION
self.current_target_pose = res.target_poses.current_target_pose_index
self.target_poses = res.target_poses.target_poses
self.plan_was_successful = None
self.update_ui()
def handle_next_pose(self):
res = self.handeye_client.select_target_pose(self.current_target_pose +
1)
self.current_target_pose = res.target_poses.current_target_pose_index
self.target_poses = res.target_poses.target_poses
self.plan_was_successful = None
self.state = CalibrationMovementsGUI.GOOD_STARTING_POSITION
self.update_ui()
def handle_plan(self):
self.guide_lbl.setText(
'Planning to the next position. Click on execute when a good one was found'
)
res = self.handeye_client.plan_to_selected_target_pose()
self.plan_was_successful = res.success
if self.plan_was_successful:
self.state = CalibrationMovementsGUI.GOOD_PLAN
else:
self.state = CalibrationMovementsGUI.BAD_PLAN
self.update_ui()
def handle_execute(self):
if self.plan_was_successful:
self.guide_lbl.setText('Going to the selected pose')
res = self.handeye_client.execute_plan()
if res.success:
self.state = CalibrationMovementsGUI.MOVED_TO_POSE
else:
self.state = CalibrationMovementsGUI.MOVEMENT_FAILED
self.update_ui()
class ControlModeWidget:
def __init__(self, context):
self.control_mode = 0
self.mode_pub = rospy.Publisher('/flor/controller/mode_command', VigirControlModeCommand, queue_size=10)
self._widget = context
self.vbox = QVBoxLayout()
#Add input for setting the spindle speed
list_label = QLabel("Select Control Mode")
self.vbox.addWidget(list_label)
# Indexed list of allowed control modes from feedback
self.allowed_modes = rospy.get_param("/atlas_controller/allowed_control_modes")
self.allowed_modes=rospy.get_param("/atlas_controller/allowed_control_modes")
self.mode_ids={}
self.mode_ids
for ndx,txt in enumerate(self.allowed_modes):
self.mode_ids[txt] = ndx
# API 2.13 ordering
self.bdi_mode_names=['NONE ', 'FREEZE ', 'STAND_PREP ', \
'STAND ', 'WALK ', 'STEP ', 'MANIPULATE ', \
'USER ', 'CALIBRATE ']
self.list_box = QListWidget(None)
self.list_box.addItems(self.allowed_modes)
self.list_box.currentItemChanged.connect(self.handle_selection_change)
self.vbox.addWidget(self.list_box)
self.selection_label = QLabel("Flor Selected: "+self.allowed_modes[0]+"("+str(self.control_mode)+")")
self.vbox.addWidget(self.selection_label)
self.label = QLabel("Flor Command : "+self.allowed_modes[0]+"("+str(self.control_mode)+")")
self.vbox.addWidget(self.label)
self.flor_mode = 0
self.bdi_current_mode = 0
self.bdi_desired_mode = 0
self.flor_mode_label = QLabel("Flor Mode : "+self.allowed_modes[self.flor_mode]+"("+str(self.flor_mode)+")"+" BDI:("+str(self.bdi_current_mode)+", "+str(self.bdi_desired_mode)+")")
self.vbox.addWidget(self.flor_mode_label)
#Add combo box for available settings
self.available_modes = QComboBox();
self.available_modes.addItem("");
self.available_modes.addItem("BDI");
self.available_modes.addItem("Enable Upper Body");
self.available_modes.addItem("Enable Whole Body");
self.available_modes.currentIndexChanged.connect(self.handle_avail_modes_changed)
self.vbox.addWidget(self.available_modes);
self.vbox.addStretch(1)
#Add Button for sending the behavior mode command
self.push_button = QPushButton("Set Mode")
self.push_button.clicked.connect(self.handle_set_mode)
self.vbox.addWidget(self.push_button)
self.vbox.addStretch(1)
hbox = QHBoxLayout()
hbox.addStretch(1)
self.stop_enable= QCheckBox()
self.stop_enable.setChecked(False)
hbox.addWidget(self.stop_enable)
self.stop_enable.clicked.connect(self.handle_stop_enable)
self.stop_button = QPushButton("STOP!")
self.stop_button.clicked.connect(self.handle_stop)
self.stop_button.setStyleSheet('QPushButton {background-color: gray }')
hbox.addWidget(self.stop_button)
hbox.addStretch(1)
self.vbox.addLayout(hbox)
self._widget.setLayout(self.vbox)
#add stretch at end so all GUI elements are at top of dialog
self.vbox.addStretch(1)
self.flor_mode_cmd_sub = rospy.Subscriber("/flor/controller/mode", FlorControlMode, self.florModeCallback)
def shutdown_plugin(self):
print "Shutting down ..."
self.flor_mode_cmd_sub.unregister()
self.mode_pub.unregister()
print "Done!"
# Update BDI state
def simStateCallback(self, state):
if ((self.bdi_current_state != state.current_behavior) or (self.bdi_desired_state != state.desired_behavior) or (self.bdi_error_code != state.error_code) or (self.bdi_behavior_status != state.behavior_feedback.status_flags) ):
self.bdi_current_state = state.current_behavior
self.bdi_desired_state = state.desired_behavior
self.bdi_error_code = state.error_code
self.bdi_behavior_status = state.behavior_feedback.status_flags
if ((self.bdi_current_state < 0) or (self.bdi_current_state >= length(self.bdi_mode_names))):
self.bdi_state_label.setText( " BDI State : "+self.bdi_mode_names[0]+"("+str(self.bdi_current_state)+", " + str(self.bdi_desired_state) + ") EC:("+str(self.bdi_error_code)+", " + str(self.bdi_behavior_status) +")")
else:
self.bdi_state_label.setText( " BDI State : "+self.bdi_mode_names[self.bdi_current_state]+"("+str(self.bdi_current_state)+", " + str(self.bdi_desired_state) + ") EC:("+str(self.bdi_error_code)+", " + str(self.bdi_behavior_status) +")")
def simCommandCallback(self, bdi_cmd):
if (self.bdi_behavior != bdi_cmd.behavior) :
self.bdi_behavior = bdi_cmd.behavior
self.bdi_command_label.setText(" BDI Cmd : "+self.bdi_mode_names[self.bdi_behavior]+"("+str(self.bdi_behavior)+")")
def florModeCallback(self, cmd):
if ( (self.flor_mode != cmd.control_mode) or (self.bdi_current_mode != cmd.bdi_current_behavior) or (self.bdi_desired_mode != cmd.bdi_desired_behavior) ):
if (self.flor_mode != cmd.control_mode) and (self.control_mode != cmd.control_mode):
print "Flor mode changed externally - clear selection"
self.clearSelections()
self.selection_label.setText("Flor Selected : ")
self.label.setText("Flor Command : ")
self.flor_mode = cmd.control_mode
self.bdi_current_mode = cmd.bdi_current_behavior
self.bdi_desired_mode = cmd.bdi_desired_behavior
print "Flor mode: ", self.flor_mode, " BDI: ",self.bdi_current_mode,", ",self.bdi_desired_mode
if (self.flor_mode > 250):
print "Invalid control mode "
self.flor_mode = 0
#print "Allowed modes:"
#print self.allowed_modes
self.flor_mode_label.setText(" Flor Mode : "+self.allowed_modes[self.flor_mode]+"("+str(self.flor_mode)+") BDI:("+str(self.bdi_current_mode)+","+str(self.bdi_desired_mode)+")")
def clearSelections(self):
for i in range(self.list_box.count()):
item = self.list_box.item(i)
self.list_box.setItemSelected(item, False)
self.list_box.setCurrentRow(-1)
#Slot for selecting
def handle_selection_change(self, curr, prev):
if (curr != None):
self.control_mode = self.mode_ids[curr.text()]
self.selection_label.setText("Flor Selected : "+curr.text()+"("+str(self.control_mode)+")")
#else:
# print "NULL selection"
#self.label.setText(self.allowed_modes[selected])
#self.spindle_speed_pub.publish(data=math.radians(degree_per_sec))
@Slot(bool)
def handle_set_mode(self):
print "Selected=",self.allowed_modes[self.control_mode], ' id=', self.control_mode
self.label.setText("Flor Command : "+self.allowed_modes[self.control_mode]+"("+str(self.control_mode)+")")
mode_msg = VigirControlModeCommand()
mode_msg.header.stamp = rospy.Time.now()
mode_msg.requested_control_mode = self.control_mode
print mode_msg
self.mode_pub.publish(mode_msg)
@Slot(bool)
def handle_stop(self):
# FLOR_STOP command
if (self.stop_enable.isChecked()):
self.clearSelections()
self.selection_label.setText("Flor Selected : ")
self.control_mode = self.mode_ids['stop']
print "Selected=",self.allowed_modes[self.control_mode], ' id=', self.control_mode
#self.list_box
self.selection_label.setText("Flor Selected : "+self.allowed_modes[self.control_mode]+"("+str(self.control_mode)+")")
self.label.setText("Flor Command : "+self.allowed_modes[self.control_mode]+"("+str(self.control_mode)+")")
mode_msg = VigirControlModeCommand()
mode_msg.header.stamp = rospy.Time.now()
mode_msg.requested_control_mode = self.control_mode
print mode_msg
self.mode_pub.publish(mode_msg)
else:
print "Stop disabled!"
@Slot(bool)
def handle_stop_enable(self):
if (self.stop_enable.isChecked()):
self.stop_button.setStyleSheet('QPushButton {background-color: red }')
#self.stop_enable.setChecked(False)
else:
self.stop_button.setStyleSheet('QPushButton {background-color: gray }')
#self.stop_enable.setChecked(True)
@Slot(bool)
def handle_avail_modes_changed(self, index):
print 'no longer mapping allowable modes'
return
class TextSearchFrame(QDockWidget):
'''
A frame to find text in the Editor.
'''
search_result_signal = Signal(str, bool, str, int)
''' @ivar: A signal emitted after search_threaded was started.
(search text, found or not, file, position in text)
for each result a signal will be emitted.
'''
replace_signal = Signal(str, str, int, str)
''' @ivar: A signal emitted to replace string at given position.
(search text, file, position in text, replaced by text)
'''
def __init__(self, tabwidget, parent=None):
QDockWidget.__init__(self, "Find", parent)
self.setObjectName('SearchFrame')
self.setFeatures(QDockWidget.DockWidgetMovable
| QDockWidget.DockWidgetFloatable)
self._dockwidget = QFrame(self)
self.vbox_layout = QVBoxLayout(self._dockwidget)
self.layout().setContentsMargins(0, 0, 0, 0)
self.layout().setSpacing(1)
# frame with two rows for find and replace
find_replace_frame = QFrame(self)
find_replace_vbox_layout = QVBoxLayout(find_replace_frame)
find_replace_vbox_layout.setContentsMargins(0, 0, 0, 0)
find_replace_vbox_layout.setSpacing(1)
# find_replace_vbox_layout.addSpacerItem(QSpacerItem(1, 1, QSizePolicy.Expanding, QSizePolicy.Expanding))
# create frame with find row
find_frame = self._create_find_frame()
find_replace_vbox_layout.addWidget(find_frame)
rplc_frame = self._create_replace_frame()
find_replace_vbox_layout.addWidget(rplc_frame)
# frame for find&replace and search results
self.vbox_layout.addWidget(find_replace_frame)
self.vbox_layout.addWidget(self._create_found_frame())
# self.vbox_layout.addStretch(2024)
self.setWidget(self._dockwidget)
# intern search parameters
self._tabwidget = tabwidget
self.current_search_text = ''
self.search_results = []
self.search_results_fileset = set()
self._search_result_index = -1
self._search_recursive = False
self._search_thread = None
def _create_find_frame(self):
find_frame = QFrame(self)
find_hbox_layout = QHBoxLayout(find_frame)
find_hbox_layout.setContentsMargins(0, 0, 0, 0)
find_hbox_layout.setSpacing(1)
self.search_field = EnchancedLineEdit(find_frame)
self.search_field.setPlaceholderText('search text')
self.search_field.textChanged.connect(self.on_search_text_changed)
self.search_field.returnPressed.connect(self.on_search)
find_hbox_layout.addWidget(self.search_field)
self.search_result_label = QLabel(find_frame)
self.search_result_label.setText(' ')
find_hbox_layout.addWidget(self.search_result_label)
self.find_button_back = QPushButton("<")
self.find_button_back.setFixedWidth(44)
self.find_button_back.clicked.connect(self.on_search_back)
find_hbox_layout.addWidget(self.find_button_back)
self.find_button = QPushButton(">")
self.find_button.setDefault(True)
# self.find_button.setFlat(True)
self.find_button.setFixedWidth(44)
self.find_button.clicked.connect(self.on_search)
find_hbox_layout.addWidget(self.find_button)
return find_frame
def _create_replace_frame(self):
# create frame with replace row
self.rplc_frame = rplc_frame = QFrame(self)
rplc_hbox_layout = QHBoxLayout(rplc_frame)
rplc_hbox_layout.setContentsMargins(0, 0, 0, 0)
rplc_hbox_layout.setSpacing(1)
self.replace_field = EnchancedLineEdit(rplc_frame)
self.replace_field.setPlaceholderText('replace text')
self.replace_field.returnPressed.connect(self.on_replace)
rplc_hbox_layout.addWidget(self.replace_field)
self.replace_result_label = QLabel(rplc_frame)
self.replace_result_label.setText(' ')
rplc_hbox_layout.addWidget(self.replace_result_label)
self.replace_button = replace_button = QPushButton("> &Replace >")
replace_button.setFixedWidth(90)
replace_button.clicked.connect(self.on_replace_click)
rplc_hbox_layout.addWidget(replace_button)
rplc_frame.setVisible(False)
return rplc_frame
def _create_found_frame(self):
ff_frame = QFrame(self)
self.found_files_vbox_layout = QVBoxLayout(ff_frame)
self.found_files_vbox_layout.setContentsMargins(0, 0, 0, 0)
self.recursive_search_box = QCheckBox("recursive search")
self.found_files_vbox_layout.addWidget(self.recursive_search_box)
self.found_files_list = QTreeWidget(ff_frame)
self.found_files_list.setColumnCount(1)
self.found_files_list.setFrameStyle(QFrame.StyledPanel)
self.found_files_list.setHeaderHidden(True)
self.found_files_list.itemActivated.connect(self.on_itemActivated)
self.found_files_list.setStyleSheet("QTreeWidget {"
"background-color:transparent;"
"}"
"QTreeWidget::item {"
"background-color:transparent;"
"}"
"QTreeWidget::item:selected {"
"background-color: darkgray;"
"}")
self.found_files_vbox_layout.addWidget(self.found_files_list)
self.recursive_search_box.setChecked(False)
return ff_frame
def keyPressEvent(self, event):
'''
Enable the shortcats for search and replace
'''
self.parent().keyPressEvent(event)
def on_search(self):
'''
Initiate the new search or request a next search result.
'''
if self.current_search_text != self.search_field.text(
) or self._search_recursive != self.recursive_search_box.isChecked():
# clear current search results
self._reset()
self.current_search_text = self.search_field.text()
if self.current_search_text:
path_text = {}
self._wait_for_result = True
for i in range(self._tabwidget.count()):
path_text[self._tabwidget.widget(
i).filename] = self._tabwidget.widget(
i).document().toPlainText()
self._search_recursive = self.recursive_search_box.isChecked()
self._search_thread = TextSearchThread(
self.current_search_text,
self._tabwidget.currentWidget().filename,
path_text=path_text,
recursive=self._search_recursive)
self._search_thread.search_result_signal.connect(
self.on_search_result)
self._search_thread.warning_signal.connect(
self.on_warning_result)
self._search_thread.start()
elif self.search_results:
self._check_position()
if self.search_results:
if self._search_result_index + 1 >= len(self.search_results):
self._search_result_index = -1
self._search_result_index += 1
(id, search_text, found, path, index, linenr,
line) = self.search_results[self._search_result_index]
self.search_result_signal.emit(search_text, found, path, index)
self.replace_button.setEnabled(True)
self._update_label()
def on_search_back(self):
'''
Slot to handle the search back function.
'''
self._check_position(False)
if self.search_results:
self._search_result_index -= 1
if self._search_result_index < 0:
self._search_result_index = len(self.search_results) - 1
self._update_label()
(id, search_text, found, path, index, linenr,
line) = self.search_results[self._search_result_index]
self.search_result_signal.emit(search_text, found, path, index)
self.replace_button.setEnabled(True)
def _check_position(self, forward=True):
try:
# if the position of the textCursor was changed by the user, move the search index
cur_pos = self._tabwidget.currentWidget().textCursor().position()
id, st, _f, pa, idx, lnr, ltxt = self.search_results[
self._search_result_index]
sear_pos = idx + len(st)
if cur_pos != sear_pos:
first_idx = self._get_current_index_for_current_file()
if first_idx != -1:
id, st, _f, pa, idx, lnr, ltxt = self.search_results[
first_idx]
sear_pos = idx + len(st)
while cur_pos > sear_pos and self._tabwidget.currentWidget(
).filename == pa:
first_idx += 1
id, st, _f, pa, idx, lnr, ltxt = self.search_results[
first_idx]
sear_pos = idx + len(st)
self._search_result_index = first_idx
if forward:
self._search_result_index -= 1
else:
self._reset(True)
except:
pass
def _get_current_index_for_current_file(self):
for index in range(len(self.search_results)):
id, _st, _f, pa, _idx = self.search_results[index]
if self._tabwidget.currentWidget().filename == pa:
return index
return -1
def on_search_result(self, search_text, found, path, index, linenr, line):
'''
Slot to handle the signals for search result. This signals are forwarded used
search_result_signal.
'''
if found and search_text == self.current_search_text:
id = "%d:%s" % (index, path)
self.search_results_fileset.add(path)
item = (search_text, found, path, index)
if item not in self.search_results:
self.search_results.append(
(id, search_text, found, path, index, linenr, line))
if self._wait_for_result:
self._search_result_index += 1
if index >= self._tabwidget.currentWidget().textCursor(
).position() or self._tabwidget.currentWidget(
).filename != path:
self._wait_for_result = False
self.search_result_signal.emit(search_text, found, path,
index)
self.replace_button.setEnabled(True)
pkg, rpath = package_name(os.path.dirname(path))
itemstr = '%s [%s]' % (os.path.basename(path), pkg)
if not self.found_files_list.findItems(itemstr, Qt.MatchExactly):
list_item = QTreeWidgetItem(self.found_files_list)
list_item.setText(0, itemstr)
list_item.setToolTip(0, path)
self.found_files_list.insertTopLevelItem(0, list_item)
self.found_files_list.expandAll()
for i in range(self.found_files_list.topLevelItemCount()):
top_item = self.found_files_list.topLevelItem(i)
if top_item.text(0) == itemstr:
sub_item_str = "%d: %s" % (linenr, line)
list_item2 = QTreeWidgetItem()
list_item2.setText(0, sub_item_str)
list_item2.setWhatsThis(0, id)
top_item.addChild(list_item2)
#self.found_files_list.setVisible(len(self.search_results_fileset) > 0)
self._update_label()
def on_warning_result(self, text):
rospy.logwarn(text)
def on_replace_click(self):
self.on_replace()
self.on_search()
def on_replace(self):
'''
Emits the replace signal, but only if currently selected text is equal to the searched one.
'''
if self.search_results:
try:
id, search_text, _found, path, index, linenr, line_text = self.search_results[
self._search_result_index]
cursor = self._tabwidget.currentWidget().textCursor()
if cursor.selectedText() == search_text:
rptxt = self.replace_field.text()
for rindex in range(self._search_result_index + 1,
len(self.search_results)):
iid, st, _f, pa, idx, lnr, ltxt = self.search_results[
rindex]
if path == pa:
self.search_results.pop(rindex)
self.search_results.insert(
rindex,
(iid, st, _f, pa, idx + len(rptxt) - len(st),
lnr, ltxt))
else:
break
self._remove_search_result(self._search_result_index)
self._search_result_index -= 1
self.replace_signal.emit(search_text, path, index, rptxt)
else:
self.replace_button.setEnabled(False)
except:
import traceback
print traceback.format_exc()
pass
def on_itemActivated(self, item):
'''
Go to the results for the selected file entry in the list.
'''
splits = item.whatsThis(0).split(':')
if len(splits) == 2:
item_index = int(splits[0])
item_path = splits[1]
new_search_index = -1
tmp_index = -1
search_index = -1
tmp_search_text = ''
for id, search_text, found, path, index, linenr, line_text in self.search_results:
new_search_index += 1
if item_path == path and item_index == index:
self._search_result_index = new_search_index
self.search_result_signal.emit(search_text, found, path,
index)
self._update_label()
def on_search_text_changed(self, _text):
'''
Clear search result if the text was changed.
'''
self._reset()
def _update_label(self, clear_label=False):
'''
Updates the status label for search results. The info is created from search result lists.
'''
msg = ' '
if self.search_results:
count_files = len(self.search_results_fileset)
msg = '%d/%d' % (self._search_result_index + 1,
len(self.search_results))
if count_files > 1:
msg = '%s(%d)' % (msg, count_files)
if self._search_thread is not None and self._search_thread.is_alive():
msg = 'searching..%s' % msg
elif not msg.strip() and self.current_search_text:
msg = '0 found'
self.current_search_text = ''
if clear_label:
msg = ' '
self.search_result_label.setText(msg)
self.find_button_back.setEnabled(len(self.search_results))
self._select_current_item_in_box(self._search_result_index)
def file_changed(self, path):
'''
Clears search results if for changed file are some search results are available
:param path: changed file path
:type path: str
'''
if path in self.search_results_fileset:
self._reset()
def set_replace_visible(self, value):
self.rplc_frame.setVisible(value)
self.raise_()
self.activateWindow()
if value:
self.replace_field.setFocus()
self.replace_field.selectAll()
self.setWindowTitle("Find / Replace")
else:
self.setWindowTitle("Find")
self.search_field.setFocus()
def is_replace_visible(self):
return self.rplc_frame.isVisible()
def _reset(self, force_new_search=False):
# clear current search results
if self._search_thread is not None:
self._search_thread.search_result_signal.disconnect()
self._search_thread.stop()
self._search_thread = None
self.current_search_text = ''
self.search_results = []
self.search_results_fileset = set()
self.found_files_list.clear()
# self.found_files_list.setVisible(False)
self._update_label(True)
self._search_result_index = -1
self.find_button_back.setEnabled(False)
if force_new_search:
self.on_search()
def enable(self):
self.setVisible(True)
# self.show()
self.raise_()
self.activateWindow()
self.search_field.setFocus()
self.search_field.selectAll()
def _select_current_item_in_box(self, index):
try:
(id, search_text, found, path, index, linenr,
line) = self.search_results[index]
for topidx in range(self.found_files_list.topLevelItemCount()):
topitem = self.found_files_list.topLevelItem(topidx)
for childdx in range(topitem.childCount()):
child = topitem.child(childdx)
if child.whatsThis(0) == id:
child.setSelected(True)
elif child.isSelected():
child.setSelected(False)
except:
pass
def _remove_search_result(self, index):
try:
(id, search_text, found, path, index, linenr,
line) = self.search_results.pop(index)
pkg, rpath = package_name(os.path.dirname(path))
itemstr = '%s [%s]' % (os.path.basename(path), pkg)
found_items = self.found_files_list.findItems(
itemstr, Qt.MatchExactly)
for item in found_items:
for chi in range(item.childCount()):
child = item.child(chi)
if child.whatsThis(0) == id:
item.removeChild(child)
break
# delete top level item if it is now empty
for topidx in range(self.found_files_list.topLevelItemCount()):
if self.found_files_list.topLevelItem(
topidx).childCount() == 0:
self.found_files_list.takeTopLevelItem(topidx)
break
# create new set with files contain the search text
new_path_set = set(path for _id, _st, _fd, path, _idx, lnr, lntxt
in self.search_results)
self.search_results_fileset = new_path_set
# self.found_files_list.setVisible(len(self.search_results_fileset) > 0)
except:
import traceback
print traceback.format_exc()
def __init__(self, parent, fileName, widget):
self.controllers = []
self.parent = parent
self.loadFile(fileName)
robot = URDF.from_parameter_server()
joint_list = {}
for ndx,jnt in enumerate(robot.joints):
joint_list[jnt.name] = ndx
for controller in self.controllers:
frame = QFrame()
frame.setFrameShape(QFrame.StyledPanel);
frame.setFrameShadow(QFrame.Raised);
vbox = QVBoxLayout()
label = QLabel()
label.setText(controller.label)
vbox.addWidget(label);
print controller.name
controller.snap_to_ghost_button = QPushButton("SnapGhost")
controller.snap_to_ghost_button.pressed.connect(controller.on_snapGhostPressed)
vbox.addWidget(controller.snap_to_ghost_button)
controller.snap_to_current_button = QPushButton("SnapCurrent")
controller.snap_to_current_button.pressed.connect(controller.on_snapCurrentPressed)
vbox.addWidget(controller.snap_to_current_button)
controller.apply_to_robot_button = QPushButton("ApplyRobot")
controller.apply_to_robot_button.pressed.connect(controller.on_applyRobotPressed)
vbox.addWidget(controller.apply_to_robot_button)
controller.save_joints_to_file_button = QPushButton("SaveJoints")
controller.save_joints_to_file_button.pressed.connect(controller.on_saveJointsPressed)
vbox.addWidget(controller.save_joints_to_file_button)
controller.undo_last_action_button = QPushButton("Undo Last")
controller.undo_last_action_button.pressed.connect(controller.on_undoPressed)
vbox.addWidget(controller.undo_last_action_button)
for joint in controller.joints:
label = QLabel()
label.setText(joint.name)
vbox.addWidget(label);
robot_joint = robot.joints[joint_list[joint.name]]
lower = robot_joint.limit.lower - (math.fabs(robot_joint.limit.upper)+math.fabs(robot_joint.limit.lower))*0.2
upper = robot_joint.limit.upper + (math.fabs(robot_joint.limit.upper)+math.fabs(robot_joint.limit.lower))*0.2
print " ",joint.name, " limits(", robot_joint.limit.lower,", ",robot_joint.limit.upper,") num"
joint.slider = QSlider(Qt.Horizontal)
joint.slider.setRange(int(lower*10000.0), int(upper*10000.0))
joint.slider.setValue(int(lower*10000.0))
joint.slider.setSingleStep((upper-lower)/20.0)
joint.slider.valueChanged.connect(joint.on_sliderMoved)
vbox.addWidget(joint.slider)
joint.progress_bar = QProgressBar()
joint.progress_bar.setRange(int(lower*10000.0), int(upper*10000.0))
joint.progress_bar.setValue(int(lower*10000.0))
vbox.addWidget(joint.progress_bar)
vbox.addStretch()
frame.setLayout(vbox)
widget.addWidget(frame)
class CalibrationMovementsGUI(QWidget):
NOT_INITED_YET = 0
BAD_PLAN = 1
GOOD_PLAN = 2
MOVED_TO_POSE = 3
BAD_STARTING_POSITION = 4
GOOD_STARTING_POSITION = 5
def __init__(self):
super(CalibrationMovementsGUI, self).__init__()
move_group_name = rospy.get_param('~move_group', 'manipulator')
self.angle_delta = math.radians(
rospy.get_param('~rotation_delta_degrees', 25))
self.translation_delta = rospy.get_param('~translation_delta_meters',
0.1)
max_velocity_scaling = rospy.get_param('~max_velocity_scaling', 0.5)
max_acceleration_scaling = rospy.get_param('~max_acceleration_scaling',
0.5)
self.local_mover = CalibrationMovements(move_group_name,
max_velocity_scaling,
max_acceleration_scaling)
self.current_pose = -1
self.current_plan = None
self.initUI()
self.state = CalibrationMovementsGUI.NOT_INITED_YET
def initUI(self):
self.layout = QVBoxLayout()
self.labels_layout = QHBoxLayout()
self.buttons_layout = QHBoxLayout()
self.progress_bar = QProgressBar()
self.pose_number_lbl = QLabel('0/8')
self.bad_plan_lbl = QLabel('No plan yet')
self.guide_lbl = QLabel('Hello')
self.check_start_pose_btn = QPushButton('Check starting pose')
self.check_start_pose_btn.clicked.connect(
self.handle_check_current_state)
self.next_pose_btn = QPushButton('Next Pose')
self.next_pose_btn.clicked.connect(self.handle_next_pose)
self.plan_btn = QPushButton('Plan')
self.plan_btn.clicked.connect(self.handle_plan)
self.execute_btn = QPushButton('Execute')
self.execute_btn.clicked.connect(self.handle_execute)
self.labels_layout.addWidget(self.pose_number_lbl)
self.labels_layout.addWidget(self.bad_plan_lbl)
self.buttons_layout.addWidget(self.check_start_pose_btn)
self.buttons_layout.addWidget(self.next_pose_btn)
self.buttons_layout.addWidget(self.plan_btn)
self.buttons_layout.addWidget(self.execute_btn)
self.layout.addWidget(self.progress_bar)
self.layout.addLayout(self.labels_layout)
self.layout.addWidget(self.guide_lbl)
self.layout.addLayout(self.buttons_layout)
self.setLayout(self.layout)
self.plan_btn.setEnabled(False)
self.execute_btn.setEnabled(False)
self.setWindowTitle('Local Mover')
self.show()
def updateUI(self):
self.progress_bar.setMaximum(len(self.local_mover.poses))
self.progress_bar.setValue(self.current_pose + 1)
self.pose_number_lbl.setText('{}/{}'.format(
self.current_pose + 1, len(self.local_mover.poses)))
if self.state == CalibrationMovementsGUI.BAD_PLAN:
self.bad_plan_lbl.setText('BAD plan!! Don\'t do it!!!!')
self.bad_plan_lbl.setStyleSheet('QLabel { background-color : red}')
elif self.state == CalibrationMovementsGUI.GOOD_PLAN:
self.bad_plan_lbl.setText('Good plan')
self.bad_plan_lbl.setStyleSheet(
'QLabel { background-color : green}')
else:
self.bad_plan_lbl.setText('No plan yet')
self.bad_plan_lbl.setStyleSheet('')
if self.state == CalibrationMovementsGUI.NOT_INITED_YET:
self.guide_lbl.setText(
'Bring the robot to a plausible position and check if it is a suitable starting pose'
)
elif self.state == CalibrationMovementsGUI.BAD_STARTING_POSITION:
self.guide_lbl.setText('Cannot calibrate from current position')
elif self.state == CalibrationMovementsGUI.GOOD_STARTING_POSITION:
self.guide_lbl.setText('Ready to start: click on next pose')
elif self.state == CalibrationMovementsGUI.GOOD_PLAN:
self.guide_lbl.setText(
'The plan seems good: press execute to move the robot')
elif self.state == CalibrationMovementsGUI.BAD_PLAN:
self.guide_lbl.setText('Planning failed: try again')
elif self.state == CalibrationMovementsGUI.MOVED_TO_POSE:
self.guide_lbl.setText(
'Pose reached: take a sample and go on to next pose')
can_plan = self.state == CalibrationMovementsGUI.GOOD_STARTING_POSITION
self.plan_btn.setEnabled(can_plan)
can_move = self.state == CalibrationMovementsGUI.GOOD_PLAN
self.execute_btn.setEnabled(can_move)
def handle_check_current_state(self):
self.local_mover.compute_poses_around_current_state(
self.angle_delta, self.translation_delta)
joint_limits = [math.radians(90)] * 5 + [math.radians(180)] + [
math.radians(350)
] # TODO: make param
if self.local_mover.check_poses(joint_limits):
self.state = CalibrationMovementsGUI.GOOD_STARTING_POSITION
else:
self.state = CalibrationMovementsGUI.BAD_STARTING_POSITION
self.current_pose = -1
self.updateUI()
def handle_next_pose(self):
self.guide_lbl.setText('Going to center position')
if self.current_pose != -1:
plan = self.local_mover.plan_to_start_pose()
if plan is None:
self.guide_lbl.setText(
'Failed planning to center position: try again')
else:
self.local_mover.execute_plan(plan)
if self.current_pose < len(self.local_mover.poses) - 1:
self.current_pose += 1
self.state = CalibrationMovementsGUI.GOOD_STARTING_POSITION
self.updateUI()
def handle_plan(self):
self.guide_lbl.setText(
'Planning to the next position. Click on execute when a good one was found'
)
if self.current_pose >= 0:
self.current_plan = self.local_mover.plan_to_pose(
self.local_mover.poses[self.current_pose])
if CalibrationMovements.is_crazy_plan(
self.current_plan, self.local_mover.fallback_joint_limits
): #TODO: sort out this limits story
self.state = CalibrationMovementsGUI.BAD_PLAN
else:
self.state = CalibrationMovementsGUI.GOOD_PLAN
self.updateUI()
def handle_execute(self):
if self.current_plan is not None:
self.guide_lbl.setText('Going to the selected pose')
self.local_mover.execute_plan(self.current_plan)
self.state = CalibrationMovementsGUI.MOVED_TO_POSE
self.updateUI()
class LevelSelectorPlugin(Plugin):
def __init__(self, context):
super(LevelSelectorPlugin, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('LevelSelectorPlugin')
# Create QWidget
self._widget = QWidget()
# self._widget.setFont(QFont("Times", 15, QFont.Bold))
self._button_layout = QVBoxLayout(self._widget)
self.buttons = []
self.text_label = QLabel("Waiting for MultiLevelMapData...", self._widget)
self._button_layout.addWidget(self.text_label)
self._widget.setObjectName('LevelSelectorPluginUI')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
context.add_widget(self._widget)
self.connect(self._widget, SIGNAL("update_buttons"), self.update_buttons)
self.connect(self._widget, SIGNAL("update_button_status"), self.update_button_status)
# Subcribe to the multi level map data to get information about all the maps.
self.multimap_subscriber = rospy.Subscriber("map_metadata", MultiLevelMapData, self.process_multimap)
self.levels = []
self.current_level = None
# Subscribe to the current level we are on.
self.status_subscriber = None
# Create a service proxy to change the current level.
self.level_selector_proxy = rospy.ServiceProxy("level_mux/change_current_level", ChangeCurrentLevel)
self.level_selector_proxy.wait_for_service()
def process_multimap(self, msg):
self.levels = msg.levels
self._widget.emit(SIGNAL("update_buttons"))
def update_buttons(self):
self.clean()
for index, level in enumerate(self.levels):
self.text_label.setText("Choose Level: ")
button = QPushButton(level.level_id, self._widget)
button.clicked[bool].connect(self.handle_button)
button.setCheckable(True)
self._button_layout.addWidget(button)
self.buttons.append(button)
# Subscribe to the current level we are on.
if self.status_subscriber is None:
self.status_subscriber = rospy.Subscriber("level_mux/current_level", LevelMetaData, self.process_level_status)
def update_button_status(self):
for index, level in enumerate(self.levels):
if self.current_level == level.level_id:
self.buttons[index].setChecked(True)
else:
self.buttons[index].setChecked(False)
def process_level_status(self, msg):
level_found = False
for level in self.levels:
if msg.level_id == level.level_id:
self.current_level = level.level_id
level_found = True
break
if not level_found:
self.current_level = None
self._widget.emit(SIGNAL("update_button_status"))
def handle_button(self):
source = self.sender()
if source.text() == self.current_level:
source.setChecked(True)
return
# Construct a identity pose. The level selector cannot be used to choose the initialpose, as it does not have
# the interface for specifying the position. The position should be specified via rviz.
origin_pose = PoseWithCovarianceStamped()
origin_pose.header.frame_id = frameIdFromLevelId(source.text())
origin_pose.pose.pose.orientation.w = 1 # Makes the origin quaternion valid.
origin_pose.pose.covariance[0] = 1.0
origin_pose.pose.covariance[7] = 1.0
origin_pose.pose.covariance[14] = 1.0
origin_pose.pose.covariance[21] = 1.0
origin_pose.pose.covariance[28] = 1.0
origin_pose.pose.covariance[35] = 1.0
# Don't actually publish the initial pose via the level selector. It doesn't know any better.
self.level_selector_proxy(source.text(), False, origin_pose)
def clean(self):
while self._button_layout.count():
item = self._button_layout.takeAt(0)
item.widget().deleteLater()
def save_settings(self, plugin_settings, instance_settings):
pass
def restore_settings(self, plugin_settings, instance_settings):
pass
class LocationFunction(object):
EDIT_LOCATION_PROPERITIES = 'Edit Location Properties'
ADD_LOCATION_AREA = 'Add Location'
EDIT_EXISTING_AREA = 'Edit Location'
def __init__(self, location_file, connectivity_file, map, widget,
subfunction_layout, configuration_layout, image):
self.edit_area_button = None
self.edit_area_selection_color = Qt.black
# Dictionary of polygons
self.locations = {}
# Dictionary that maps location names to their colors
self.location_colors = {}
self.draw_location = {}
self.unique_loc_counter = 1
self.editing_area = False
self.edit_existing_location = None
self.editing_properties = False
self.edit_properties_location = None
# Use this to initialize variables.
self.clearAreaSelection()
self.is_modified = False
self.widget = widget
self.subfunction_layout = subfunction_layout
self.image = image
self.image_size = image.overlay_image.size()
self.configuration_layout = configuration_layout
self.location_file = location_file
self.connectivity_file = connectivity_file
self.map_size = QSize(map.map.info.width, map.map.info.height)
self.readLocationsFromFile()
self.edit_area_button = {}
def readLocationsFromFile(self):
if os.path.isfile(self.location_file):
stream = open(self.location_file, 'r')
try:
contents = yaml.load(stream)
if "polygons" not in contents or "locations" not in contents:
rospy.logerr(
"YAML file found at " + self.location_file +
", but does not seem to have been written by this tool. I'm starting locations from scratch."
)
else:
location_keys = contents["locations"]
location_polygons = contents["polygons"]
for index, location in enumerate(location_keys):
self.locations[location] = QPolygon()
self.locations[location].setPoints(
location_polygons[index])
self.locations[location] = scalePolygon(
self.locations[location], self.map_size,
self.image_size)
(_, self.location_colors[location]
) = self.getUniqueNameAndColor()
self.draw_location[location] = True
except yaml.YAMLError:
rospy.logerr(
"File found at " + self.location_file +
", but cannot be parsed by YAML parser. I'm starting locations from scratch."
)
stream.close()
else:
rospy.logwarn(
"Location file not found at " + self.location_file +
". I'm starting locations from scratch and will attempt to write to this location before exiting."
)
def saveConfiguration(self):
self.writeLocationsToFile()
self.writeConnectivityToFile()
def writeConnectivityToFile(self):
connectivity = makeConnectivityMap(self.locations.values())
out_dict = {}
poly_to_name = {poly: name for name, poly in self.locations.items()}
for location_poly, neighbor_poly in connectivity.items():
sorted_list = sorted([poly_to_name[p] for p in neighbor_poly])
out_dict[poly_to_name[location_poly]] = sorted_list
stream = open(self.connectivity_file, 'w')
yaml.dump(out_dict, stream)
stream.close()
def writeLocationsToFile(self):
out_dict = {}
out_dict["locations"] = self.locations.keys()
out_dict["locations"] = sorted(out_dict["locations"])
out_dict["polygons"] = []
for index, key in enumerate(sorted(self.locations)):
out_dict["polygons"].append([])
polygon = self.locations[key]
for i in range(polygon.size()):
pt = polygon.point(i)
scaled_pt = scalePoint(pt, self.image_size, self.map_size)
out_dict["polygons"][index].append(scaled_pt.x())
out_dict["polygons"][index].append(scaled_pt.y())
data_directory = os.path.dirname(os.path.realpath(self.location_file))
image_file = getLocationsImageFileLocationFromDataDirectory(
data_directory)
# Create an image with the location data, so that C++ programs don't need to rely on determining regions using polygons.
out_dict["data"] = 'locations.pgm'
location_image = QImage(self.map_size, QImage.Format_RGB32)
location_image.fill(Qt.white)
painter = QPainter(location_image)
for index, key in enumerate(self.locations):
if index > 254:
rospy.logerr(
"You have more than 254 locations, which is unsupported by the bwi_planning_common C++ code!"
)
painter.setPen(Qt.NoPen)
painter.setBrush(QColor(index, index, index))
scaled_polygon = scalePolygon(self.locations[key], self.image_size,
self.map_size)
painter.drawPolygon(scaled_polygon)
painter.end()
location_image.save(image_file)
stream = open(self.location_file, 'w')
yaml.dump(out_dict, stream)
stream.close()
self.is_modified = False
def deactivateFunction(self):
if self.editing_area:
self.endAreaEdit("Cancel")
elif self.editing_properties:
self.endPropertyEdit()
clearLayoutAndFixHeight(self.subfunction_layout)
self.edit_area_button.clear()
self.image.enableDefaultMouseHooks()
# Just in case we were editing a location, that location was not being drawn.
for location in self.draw_location:
self.draw_location[location] = True
def activateFunction(self):
# Add all the necessary buttons to the subfunction layout.
clearLayoutAndFixHeight(self.subfunction_layout)
for button_text in [
LocationFunction.ADD_LOCATION_AREA,
LocationFunction.EDIT_EXISTING_AREA
]:
button = QPushButton(button_text, self.widget)
button.clicked[bool].connect(
partial(self.startAreaEdit, button_text))
button.setCheckable(True)
self.subfunction_layout.addWidget(button)
self.edit_area_button[button_text] = button
self.edit_area_button[LocationFunction.EDIT_EXISTING_AREA].setEnabled(
False)
self.subfunction_layout.addStretch(1)
# ActivateMouseHooks.
self.image.mousePressEvent = self.mousePressEvent
self.image.mouseMoveEvent = self.mouseMoveEvent
self.image.mouseReleaseEvent = self.mouseReleaseEvent
self.updateOverlay()
def getLocationNameFromPoint(self, point):
for location in self.locations:
if self.locations[location].containsPoint(point, Qt.OddEvenFill):
return location
return None
def startAreaEdit(self, edit_type):
if self.editing_properties:
self.endPropertyEdit()
self.editing_area = True
if edit_type == LocationFunction.ADD_LOCATION_AREA:
self.edit_existing_location = None
# else edit_existing_location was set to the correct location by startPropertyEdit()
# Make sure all active selections have been cleared.
self.clearAreaSelection()
# If we're going to edit an existing area, stop drawing it and copy it to the active selection.
if self.edit_existing_location is not None:
self.draw_location[self.edit_existing_location] = False
self.current_selection = QPolygon(
self.locations[self.edit_existing_location])
self.edit_existing_location = self.edit_existing_location
# Setup the buttons in the configuration toolbar, and disable the original buttons to edit an area.
clearLayoutAndFixHeight(self.configuration_layout)
for button_text in ["Done", "Cancel"]:
button = QPushButton(button_text, self.widget)
button.clicked[bool].connect(partial(self.endAreaEdit,
button_text))
self.configuration_layout.addWidget(button)
self.current_selection_label = QLabel(self.widget)
self.configuration_layout.addWidget(self.current_selection_label)
self.configuration_layout.addStretch(1)
self.edit_area_button[LocationFunction.ADD_LOCATION_AREA].setEnabled(
False)
self.edit_area_button[LocationFunction.EDIT_EXISTING_AREA].setEnabled(
False)
self.updateOverlay()
def clearAreaSelection(self):
# Make sure all selections are clear.
self.new_selection_start_point = None
self.new_selection_end_point = None
# QPolygons to track current location.
self.current_selection = None
self.new_selection = None
self.current_selection_label = None
self.subtract_new_selection = None
def endAreaEdit(self, button_text):
edit_properties_location = None
if (button_text
== "Done") and (self.current_selection is not None) and (
not self.current_selection.isEmpty()):
# If the current location being added completely wipes out an old location, make sure you remove it.
for location in self.locations.keys():
if location != self.edit_existing_location:
self.locations[location] = self.locations[
location].subtracted(self.current_selection)
if self.locations[location].isEmpty():
self.removeLocation(location)
if self.edit_existing_location == None:
# We're adding a new location. Generate a new location name and color.
(self.edit_existing_location,
new_location_color) = self.getUniqueNameAndColor()
self.location_colors[
self.edit_existing_location] = new_location_color
self.locations[
self.edit_existing_location] = self.current_selection
self.draw_location[self.edit_existing_location] = True
edit_properties_location = self.edit_existing_location
# Since a location was added or edited, set file modification to true.
self.is_modified = True
else:
# Cancel was pressed, draw the original location if we were editing as before.
if self.edit_existing_location is not None:
self.draw_location[self.edit_existing_location] = True
self.editing_area = False
self.edit_existing_location = None
self.clearAreaSelection()
# Update the entire image overlay.
self.updateOverlay()
self.edit_area_button[LocationFunction.ADD_LOCATION_AREA].setEnabled(
True)
self.edit_area_button[LocationFunction.ADD_LOCATION_AREA].setChecked(
False)
self.edit_area_button[LocationFunction.EDIT_EXISTING_AREA].setChecked(
False)
clearLayoutAndFixHeight(self.configuration_layout)
if edit_properties_location is not None:
self.edit_properties_location = edit_properties_location
self.startPropertyEdit()
def startPropertyEdit(self):
self.editing_properties = True
self.edit_existing_location = self.edit_properties_location
self.edit_area_button[LocationFunction.ADD_LOCATION_AREA].setEnabled(
True)
self.edit_area_button[LocationFunction.EDIT_EXISTING_AREA].setEnabled(
True)
# Construct the configuration layout.
clearLayoutAndFixHeight(self.configuration_layout)
neighbors_str = makeNeighborsString(
self.locations[self.edit_properties_location], self.locations)
self.update_name_label = QLabel(
"Location (" + self.edit_properties_location + " - " +
neighbors_str + ") New Name: ", self.widget)
self.configuration_layout.addWidget(self.update_name_label)
self.update_name_textedit = QLineEdit(self.widget)
self.update_name_textedit.setText(self.edit_properties_location)
self.update_name_textedit.textEdited.connect(
self.locationNameTextEdited)
self.configuration_layout.addWidget(self.update_name_textedit)
self.update_name_button = QPushButton("Update location Name",
self.widget)
self.update_name_button.clicked[bool].connect(self.updateLocationName)
self.update_name_button.setEnabled(False)
self.configuration_layout.addWidget(self.update_name_button)
self.remove_location_button = QPushButton("Remove Location",
self.widget)
self.remove_location_button.clicked[bool].connect(
self.removeCurrentLocation)
self.configuration_layout.addWidget(self.remove_location_button)
self.configuration_layout.addStretch(1)
self.updateOverlay()
def endPropertyEdit(self):
self.edit_area_button[LocationFunction.ADD_LOCATION_AREA].setEnabled(
True)
self.edit_area_button[LocationFunction.EDIT_EXISTING_AREA].setEnabled(
False)
clearLayoutAndFixHeight(self.configuration_layout)
self.update_name_label = None
self.update_name_textedit = None
self.update_name_button = None
self.editing_properties = False
self.edit_properties_location = None
self.updateOverlay()
def locationNameTextEdited(self, text):
if str(text) != self.edit_properties_location:
self.update_name_button.setEnabled(True)
else:
self.update_name_button.setEnabled(False)
def updateLocationName(self):
old_loc_name = self.edit_properties_location
new_loc_name = str(self.update_name_textedit.text())
if new_loc_name in self.locations:
# This means that two locations need to be merged
self.locations[new_loc_name] = self.locations[new_loc_name].united(
self.locations.pop(old_loc_name))
else:
# This is a simple rename task.
self.locations[new_loc_name] = self.locations.pop(old_loc_name)
self.location_colors[new_loc_name] = self.location_colors.pop(
old_loc_name)
self.draw_location[new_loc_name] = self.draw_location.pop(
old_loc_name)
# Since a location name was modified, set file modification to true.
self.is_modified = True
# Restart property edit with the updated name.
self.endPropertyEdit()
self.edit_properties_location = new_loc_name
self.startPropertyEdit()
def removeCurrentLocation(self):
old_loc_name = self.edit_properties_location
self.removeLocation(old_loc_name)
self.endPropertyEdit()
self.updateOverlay()
# Since a location was removed, set file modification to true.
self.is_modified = True
def removeLocation(self, loc_name):
if loc_name in self.locations:
self.locations.pop(loc_name)
if loc_name in self.location_colors:
self.location_colors.pop(loc_name)
if loc_name in self.draw_location:
self.draw_location.pop(loc_name)
def isModified(self):
return self.is_modified
def mousePressEvent(self, event):
if self.editing_area:
self.subtract_new_selection = event.button() == Qt.RightButton
self.new_selection_start_point = event.pos()
self.new_selection_end_point = event.pos()
self.new_selection = None
else:
loc = self.getLocationNameFromPoint(event.pos())
if loc is not None:
self.edit_properties_location = loc
self.startPropertyEdit()
else:
self.endPropertyEdit()
def mouseReleaseEvent(self, event):
if self.editing_area:
self.mouseMoveEvent(event)
locations_sans_selection = copy.copy(self.locations)
if self.new_selection is not None:
if self.current_selection is None and self.subtract_new_selection is False:
self.current_selection = self.new_selection
if self.subtract_new_selection:
self.current_selection = self.current_selection.subtracted(
self.new_selection)
else:
self.current_selection = self.current_selection.united(
self.new_selection)
if self.edit_existing_location:
del locations_sans_selection[self.edit_existing_location]
self.new_selection = None
self.subtract_new_selection = None
neighbor_str = makeNeighborsString(self.current_selection,
locations_sans_selection)
self.current_selection_label.setText(neighbor_str)
def mouseMoveEvent(self, event):
if self.editing_area:
# First make sure we update the region corresponding to the old mark.
old_overlay_update_rect = self.get_rectangular_polygon(
self.new_selection_start_point, self.new_selection_end_point)
# Draw new mark, taking some care to reduce the size of the polygon's bottom right corner by (1,1).
self.new_selection_end_point = event.pos()
self.new_selection = self.get_rectangular_polygon(
self.new_selection_start_point, self.new_selection_end_point)
self.new_selection = self.new_selection.boundingRect()
self.new_selection.setHeight(self.new_selection.height() - 1)
self.new_selection.setWidth(self.new_selection.width() - 1)
self.new_selection = QPolygon(self.new_selection, True)
# Next determine the region that needs to be update because of the new mark.
new_overlay_update_rect = self.get_rectangular_polygon(
self.new_selection_start_point, self.new_selection_end_point)
overlay_update_region = (old_overlay_update_rect +
new_overlay_update_rect).boundingRect()
self.updateOverlay(overlay_update_region)
def updateOverlay(self, rect=None):
# Redraw the overlay image from scratch using the location image and current location.
self.image.overlay_image.fill(Qt.transparent)
painter = QPainter(self.image.overlay_image)
painter.setBackgroundMode(Qt.TransparentMode)
painter.setCompositionMode(QPainter.CompositionMode_Source)
for location in self.locations:
if self.draw_location[location]:
color = self.location_colors[location]
if self.edit_properties_location == location and self.editing_properties:
color = self.edit_area_selection_color
lineColor = QColor(color)
lineColor.setAlpha(255)
brushColor = QColor(color)
brushColor.setAlpha(128)
painter.setPen(lineColor)
painter.setBrush(brushColor)
painter.drawPolygon(self.locations[location])
if (self.current_selection is not None) or (self.new_selection
is not None):
lineColor = QColor(self.edit_area_selection_color)
lineColor.setAlpha(255)
brushColor = QColor(self.edit_area_selection_color)
brushColor.setAlpha(128)
painter.setPen(lineColor)
painter.setBrush(brushColor)
if self.new_selection is not None:
# Create a temporary polygon as the new selection is being drawn.
if self.current_selection is not None:
current_selection = QPolygon(self.current_selection)
if self.subtract_new_selection:
current_selection = current_selection.subtracted(
self.new_selection)
else:
current_selection = current_selection.united(
self.new_selection)
painter.drawPolygon(current_selection)
elif self.subtract_new_selection == False:
painter.drawPolygon(self.new_selection)
else:
painter.drawPolygon(self.current_selection)
painter.end()
if rect is None:
self.image.update()
else:
self.image.update(rect)
def getUniqueNameAndColor(self):
"""
Use golden ratio to generate unique colors.
http://martin.ankerl.com/2009/12/09/how-to-create-random-colors-programmatically/
"""
name = "new_loc" + str(self.unique_loc_counter)
h = int(359 * (self.unique_loc_counter * 0.618033988749895))
h = h % 359
self.unique_loc_counter += 1
return name, QColor.fromHsv(h, 255, 255)
def get_rectangular_polygon(self, pt1, pt2):
return QPolygon(
[pt1, QPoint(pt1.x(), pt2.y()), pt2,
QPoint(pt2.x(), pt1.y())])
class PingGUI(Plugin):
def __init__(self, context):
super(PingGUI, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName("PingGUI")
self.msg = None
# Create a container widget and give it a layout
self._container = QWidget()
self._layout = QVBoxLayout()
self._container.setLayout(self._layout)
self._label = QLabel("xx ms latency")
p = self._label.palette()
p.setColor(self._label.backgroundRole(), Qt.red)
self._label.setPalette(p)
self._layout.addWidget(self._label)
self.set_bg_color(100, 100, 100)
rospy.Subscriber("/ping/delay", Float64, self.ping_cb)
context.add_widget(self._container)
self._update_plot_timer = QTimer(self)
self._update_plot_timer.timeout.connect(self.update_gui)
self._update_plot_timer.start(1)
def update_gui(self):
if not self.msg:
return
msg = self.msg
# msec
# 100 -> green, 1000 -> red
# normalize within 1000 ~ 100
orig_latency = msg.data
msg_data = orig_latency
if msg.data > 1000:
msg_data = 1000
elif msg.data < 100:
msg_data = 100
ratio = (msg_data - 100) / (1000 - 100)
color_r = ratio * 255.0
color_g = (1 - ratio) * 255.0
# devnull = open(os.devnull, "w")
# with RedirectStdStreams(stdout=devnull, stderr=devnull):
self.set_bg_color(color_r, color_g, 0)
self._label.setText("%d ms latency" % (orig_latency))
def set_bg_color(self, r, g, b):
self._label.setStyleSheet(
"QLabel { display: block; background-color: rgba(%d, %d, %d, 255); text-align: center; font-size: 30px;}"
% (r, g, b)
)
def ping_cb(self, msg):
self.msg = msg
def shutdown_plugin(self):
pass
def save_settings(self, plugin_settings, instance_settings):
pass
def restore_settings(self, plugin_settings, instance_settings):
pass
class Editor(QMainWindow):
'''
Creates a dialog to edit a launch file.
'''
finished_signal = Signal(list)
'''
finished_signal has as parameter the filenames of the initialization and is emitted, if this
dialog was closed.
'''
def __init__(self, filenames, search_text='', master_name='', parent=None):
'''
:param filenames: a list with filenames. The last one will be activated.
:type filenames: [str]
:param str search_text: if not empty, searches in new document for first occurrence of the given text
'''
QMainWindow.__init__(self, parent)
self.setObjectName('Editor - %s' % utf8(filenames))
self.setAttribute(Qt.WA_DeleteOnClose, True)
self.setWindowFlags(Qt.Window)
self.mIcon = nm.settings().icon('crystal_clear_edit_launch.png')
self._error_icon = nm.settings().icon('warning.png')
self._info_icon = nm.settings().icon('info.png')
self._empty_icon = QIcon()
self.setWindowIcon(self.mIcon)
window_title = "ROSLaunch Editor"
if filenames:
window_title = self.__getTabName(filenames[0])
self.setWindowTitle('%s @%s' % (window_title, master_name))
self.init_filenames = filenames
self._search_node_count = 0
self._search_thread = None
self._last_search_request = None
# list with all open files
self.files = []
# create tabs for files
self.main_widget = QWidget(self)
self.main_widget.setObjectName("editorMain")
self.verticalLayout = QVBoxLayout(self.main_widget)
self.verticalLayout.setContentsMargins(0, 0, 0, 0)
self.verticalLayout.setSpacing(1)
self.verticalLayout.setObjectName("verticalLayout")
self.tabWidget = EditorTabWidget(self)
self.tabWidget.setTabPosition(QTabWidget.North)
self.tabWidget.setDocumentMode(True)
self.tabWidget.setTabsClosable(True)
self.tabWidget.setMovable(False)
self.tabWidget.setObjectName("tabWidget")
self.tabWidget.tabCloseRequested.connect(self.on_close_tab)
self.tabWidget.currentChanged.connect(self.on_tab_changed)
self.verticalLayout.addWidget(self.tabWidget)
self.log_dock = self._create_log_bar()
self.addDockWidget(Qt.BottomDockWidgetArea, self.log_dock)
# self.verticalLayout.addWidget(self.log_bar)
self.buttons = self._create_buttons()
self.verticalLayout.addWidget(self.buttons)
self.setCentralWidget(self.main_widget)
self.find_dialog = TextSearchFrame(self.tabWidget, self)
self.find_dialog.found_signal.connect(self.on_search_result)
self.find_dialog.replace_signal.connect(self.on_replace)
self.addDockWidget(Qt.RightDockWidgetArea, self.find_dialog)
self.graph_view = GraphViewWidget(self.tabWidget, self)
self.graph_view.load_signal.connect(self.on_graph_load_file)
self.graph_view.goto_signal.connect(self.on_graph_goto)
self.graph_view.search_signal.connect(self.on_load_request)
self.graph_view.finished_signal.connect(self.on_graph_finished)
self.graph_view.info_signal.connect(self.on_graph_info)
self.addDockWidget(Qt.RightDockWidgetArea, self.graph_view)
self.readSettings()
self.find_dialog.setVisible(False)
self.graph_view.setVisible(False)
nm.nmd().file.changed_file.connect(self.on_changed_file)
nm.nmd().file.packages_available.connect(self._on_new_packages)
# open the files
for f in filenames:
if f:
self.on_load_request(f, search_text, only_launch=True)
self.log_dock.setVisible(False)
try:
pal = self.tabWidget.palette()
self._default_color = pal.color(QPalette.Window)
color = QColor.fromRgb(nm.settings().host_color(master_name, self._default_color.rgb()))
bg_style_launch_dock = "QWidget#editorMain { background-color: qlineargradient(x1: 0, y1: 0, x2: 0, y2: 1, stop: 0 %s, stop: 0.7 %s);}" % (color.name(), self._default_color.name())
self.setStyleSheet('%s' % (bg_style_launch_dock))
except Exception as _:
pass
# import traceback
# print(traceback.format_exc())
# def __del__(self):
# print "******** destroy", self.objectName()
def _create_buttons(self):
# create the buttons line
self.buttons = QWidget(self)
self.horizontalLayout = QHBoxLayout(self.buttons)
self.horizontalLayout.setContentsMargins(3, 0, 3, 0)
self.horizontalLayout.setObjectName("horizontalLayout")
# add open upper launchfile button
self.upperButton = QPushButton(self)
self.upperButton.setObjectName("upperButton")
self.upperButton.clicked.connect(self.on_upperButton_clicked)
self.upperButton.setIcon(nm.settings().icon('up.png'))
self.upperButton.setShortcut("Ctrl+U")
self.upperButton.setToolTip('Open the file which include the current file (Ctrl+U)')
self.upperButton.setFlat(True)
self.horizontalLayout.addWidget(self.upperButton)
# add the goto button
self.gotoButton = QPushButton(self)
self.gotoButton.setObjectName("gotoButton")
self.gotoButton.clicked.connect(self.on_shortcut_goto)
self.gotoButton.setText(self._translate("&Goto line"))
self.gotoButton.setShortcut("Ctrl+G")
self.gotoButton.setToolTip('Open a goto dialog (Ctrl+G)')
self.gotoButton.setFlat(True)
self.horizontalLayout.addWidget(self.gotoButton)
# add a tag button
self.tagButton = self._create_tag_button(self)
self.horizontalLayout.addWidget(self.tagButton)
# add save button
self.saveButton = QPushButton(self)
self.saveButton.setObjectName("saveButton")
self.saveButton.setIcon(QIcon.fromTheme("document-save"))
self.saveButton.clicked.connect(self.on_saveButton_clicked)
self.saveButton.setText(self._translate("&Save"))
self.saveButton.setShortcut("Ctrl+S")
self.saveButton.setToolTip('Save the changes to the file (Ctrl+S)')
self.saveButton.setFlat(True)
self.horizontalLayout.addWidget(self.saveButton)
# add spacer
spacerItem = QSpacerItem(515, 20, QSizePolicy.Expanding, QSizePolicy.Minimum)
self.horizontalLayout.addItem(spacerItem)
# add line number label
self.pos_label = QLabel()
self.horizontalLayout.addWidget(self.pos_label)
# add spacer
spacerItem = QSpacerItem(515, 20, QSizePolicy.Expanding, QSizePolicy.Minimum)
self.horizontalLayout.addItem(spacerItem)
# add show log button
self.show_log_button = QPushButton("Log>>", self)
self.show_log_button.setObjectName("show_log_button")
self.show_log_button.clicked.connect(self.on_toggled_log)
self.show_log_button.setFlat(True)
self.show_log_button.setCheckable(True)
self.horizontalLayout.addWidget(self.show_log_button)
# add graph button
self.graphButton = QPushButton(self)
self.graphButton.setObjectName("graphButton")
self.graphButton.toggled.connect(self.on_toggled_graph)
self.graphButton.setText("Includ&e Graph >>")
self.graphButton.setCheckable(True)
self.graphButton.setShortcut("Ctrl+E")
self.graphButton.setToolTip('Shows include and include from files (Ctrl+E)')
self.graphButton.setFlat(True)
self.horizontalLayout.addWidget(self.graphButton)
# add the search button
self.searchButton = QPushButton(self)
self.searchButton.setObjectName("searchButton")
# self.searchButton.clicked.connect(self.on_shortcut_find)
self.searchButton.toggled.connect(self.on_toggled_find)
self.searchButton.setText(self._translate("&Find >>"))
self.searchButton.setToolTip('Open a search dialog (Ctrl+F)')
self.searchButton.setFlat(True)
self.searchButton.setCheckable(True)
self.horizontalLayout.addWidget(self.searchButton)
# add the replace button
self.replaceButton = QPushButton(self)
self.replaceButton.setObjectName("replaceButton")
# self.replaceButton.clicked.connect(self.on_shortcut_replace)
self.replaceButton.toggled.connect(self.on_toggled_replace)
self.replaceButton.setText(self._translate("&Replace >>"))
self.replaceButton.setToolTip('Open a search&replace dialog (Ctrl+R)')
self.replaceButton.setFlat(True)
self.replaceButton.setCheckable(True)
self.horizontalLayout.addWidget(self.replaceButton)
return self.buttons
def _create_log_bar(self):
self.log_dock = QDockWidget(self)
self.log_dock.setObjectName('LogFrame')
self.log_dock.setFeatures(QDockWidget.DockWidgetMovable | QDockWidget.DockWidgetFloatable)
self.log_bar = QWidget(self)
self.horizontal_layout_log_bar = QHBoxLayout(self.log_bar)
self.horizontal_layout_log_bar.setContentsMargins(2, 0, 2, 0)
self.horizontal_layout_log_bar.setObjectName("horizontal_layout_log_bar")
# add info label
self._log_warning_count = 0
self.log_browser = QTextEdit()
self.log_browser.setObjectName("log_browser")
self.log_browser.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding)
self.log_browser.setLineWrapMode(QTextEdit.NoWrap)
# self.log_browser.setMaximumHeight(120)
color = QColor(255, 255, 235)
bg_style = "QTextEdit#log_browser { background-color: %s;}" % color.name()
self.log_bar.setStyleSheet("%s" % (bg_style))
self.horizontal_layout_log_bar.addWidget(self.log_browser)
# add hide button
self.clear_log_button = QPushButton("clear", self)
self.clear_log_button.setObjectName("clear_log_button")
self.clear_log_button.setSizePolicy(QSizePolicy.Minimum, QSizePolicy.Maximum)
self.clear_log_button.clicked.connect(self.on_clear_log_button_clicked)
self.clear_log_button.setFlat(True)
self.horizontal_layout_log_bar.addWidget(self.clear_log_button)
self.log_dock.setWidget(self.log_bar)
return self.log_dock
def keyPressEvent(self, event):
'''
Enable the shortcats for search and replace
'''
if event.key() == Qt.Key_Escape:
self.reject()
elif event.modifiers() == Qt.ControlModifier and event.key() == Qt.Key_F:
if self.tabWidget.currentWidget().hasFocus():
if not self.searchButton.isChecked():
self.searchButton.setChecked(True)
else:
self.on_toggled_find(True)
else:
self.searchButton.setChecked(not self.searchButton.isChecked())
elif event.modifiers() == Qt.ControlModifier and event.key() == Qt.Key_R:
if self.tabWidget.currentWidget().hasFocus():
if not self.replaceButton.isChecked():
self.replaceButton.setChecked(True)
else:
self.on_toggled_replace(True)
else:
self.replaceButton.setChecked(not self.replaceButton.isChecked())
elif event.modifiers() == Qt.ControlModifier and event.key() == Qt.Key_E:
if self.tabWidget.currentWidget().hasFocus():
if not self.graphButton.isChecked():
self.graphButton.setChecked(True)
else:
self.on_toggled_graph(True)
else:
self.graphButton.setChecked(not self.graphButton.isChecked())
elif event.modifiers() == Qt.ControlModifier and event.key() == Qt.Key_W:
self.on_close_tab(self.tabWidget.currentIndex())
elif event.modifiers() in [Qt.ControlModifier, Qt.AltModifier] and event.key() == Qt.Key_Up:
self.on_upperButton_clicked()
elif event.modifiers() in [Qt.ControlModifier, Qt.AltModifier] and event.key() == Qt.Key_Down:
self.on_downButton_clicked()
else:
event.accept()
QMainWindow.keyPressEvent(self, event)
def _translate(self, text):
if hasattr(QApplication, "UnicodeUTF8"):
return QApplication.translate("Editor", text, None, QApplication.UnicodeUTF8)
else:
return QApplication.translate("Editor", text, None)
def readSettings(self):
if nm.settings().store_geometry:
settings = nm.settings().qsettings(nm.settings().CFG_GUI_FILE)
settings.beginGroup("editor")
maximized = settings.value("maximized", 'false') == 'true'
if maximized:
self.showMaximized()
else:
self.resize(settings.value("size", QSize(800, 640)))
self.move(settings.value("pos", QPoint(0, 0)))
try:
self.restoreState(settings.value("window_state"))
except Exception:
import traceback
print(traceback.format_exc())
settings.endGroup()
def storeSetting(self):
if nm.settings().store_geometry:
settings = nm.settings().qsettings(nm.settings().CFG_GUI_FILE)
settings.beginGroup("editor")
settings.setValue("size", self.size())
settings.setValue("pos", self.pos())
settings.setValue("maximized", self.isMaximized())
settings.setValue("window_state", self.saveState())
settings.endGroup()
def on_load_request(self, filename, search_text='', insert_index=-1, goto_line=-1, only_launch=False, count_results=0):
'''
Loads a file in a new tab or focus the tab, if the file is already open.
:param str filename: the path to file
:param str search_text: if not empty, searches in new document for first occurrence of the given text
'''
if not filename:
return
self.tabWidget.setUpdatesEnabled(False)
try:
if filename not in self.files:
tab_name = self.__getTabName(filename)
editor = TextEdit(filename, parent=self)
linenumber_editor = LineNumberWidget(editor)
tab_index = 0
if insert_index > -1:
tab_index = self.tabWidget.insertTab(insert_index, linenumber_editor, tab_name)
else:
tab_index = self.tabWidget.addTab(linenumber_editor, tab_name)
self.files.append(filename)
editor.setCurrentPath(os.path.basename(filename))
editor.load_request_signal.connect(self.on_load_request)
editor.document().modificationChanged.connect(self.on_editor_modificationChanged)
editor.cursorPositionChanged.connect(self.on_editor_positionChanged)
editor.setFocus(Qt.OtherFocusReason)
# editor.textChanged.connect(self.on_text_changed)
editor.undoAvailable.connect(self.on_text_changed)
self.tabWidget.setCurrentIndex(tab_index)
# self.find_dialog.set_search_path(filename)
else:
for i in range(self.tabWidget.count()):
if self.tabWidget.widget(i).filename == filename:
self.tabWidget.setCurrentIndex(i)
break
self.tabWidget.setUpdatesEnabled(True)
if search_text:
if only_launch:
self.find_dialog.found_files_list.clear()
try:
self._search_thread.stop()
self._search_thread = None
except Exception:
pass
# TODO: put all text of all tabs into path_text
rospy.logdebug("serach for '%s'" % search_text)
self._search_node_count = 0
self._search_thread = TextSearchThread(search_text, filename, recursive=True, only_launch=only_launch, count_results=count_results)
self._search_thread.search_result_signal.connect(self.on_search_result_on_open)
self._search_thread.warning_signal.connect(self.on_search_result_warning)
self._last_search_request = (filename, search_text, insert_index, goto_line, only_launch)
if not self.graph_view.is_loading():
self.on_graph_info("search thread: start search for '%s'" % self._search_thread._search_text)
self._search_thread.start()
if goto_line != -1:
self._goto(goto_line, True)
self.upperButton.setEnabled(self.tabWidget.count() > 1)
except Exception as err:
self.tabWidget.setUpdatesEnabled(True)
import traceback
msg = "Error while open %s: %s" % (filename, traceback.format_exc())
rospy.logwarn(msg)
MessageBox.critical(self, "Error", utf8(err), msg)
if self.tabWidget.count() == 0:
self.close()
def on_graph_load_file(self, path, insert_after=True):
insert_index = self.tabWidget.currentIndex() + 1
if not insert_after and insert_index > 1:
insert_index = self.tabWidget.currentIndex()
self.on_load_request(path, insert_index=insert_index)
def on_graph_goto(self, path, linenr):
if path == self.tabWidget.currentWidget().filename:
if linenr != -1:
self._goto(linenr, True)
def on_graph_finished(self):
self.on_graph_info("build tree: finished", False)
if self.graph_view.has_warnings:
self.graphButton.setIcon(self._info_icon)
else:
self.graphButton.setIcon(self._empty_icon)
if self._search_thread:
try:
self._search_thread.find_args_not_set = True
self._search_thread.start()
self.on_graph_info("search thread: start search for '%s'" % self._search_thread._search_text)
except Exception:
pass
def on_graph_info(self, msg, warning=False):
text_color = "#000000"
if warning:
self._log_warning_count += 1
if self._log_warning_count == 1:
self.show_log_button.setIcon(self._error_icon)
text_color = "#FE9A2E"
text = ('<pre style="padding:10px;"><dt><font color="%s">'
'%s</font></dt></pre>' % (text_color, msg))
self.log_browser.append(text)
def on_text_changed(self, value=""):
if self.tabWidget.currentWidget().hasFocus():
self.find_dialog.file_changed(self.tabWidget.currentWidget().filename)
self._last_search_request = None
def on_tab_changed(self, index):
if index > -1:
self.graph_view.set_file(self.tabWidget.widget(index).filename, self.tabWidget.widget(0).filename)
self._last_search_request = None
def on_close_tab(self, tab_index):
'''
Signal handling to close single tabs.
:param int tab_index: tab index to close
'''
try:
doremove = True
w = self.tabWidget.widget(tab_index)
if w.document().isModified():
name = self.__getTabName(w.filename)
result = MessageBox.question(self, "Unsaved Changes", '\n\n'.join(["Save the file before closing?", name]))
if result == MessageBox.Yes:
self.tabWidget.currentWidget().save()
elif result == MessageBox.No:
pass
elif rospy.is_shutdown():
doremove = False
if doremove:
# remove the indexed files
if w.filename in self.files:
self.files.remove(w.filename)
# close tab
self.tabWidget.removeTab(tab_index)
# close editor, if no tabs are open
if not self.tabWidget.count():
self.close()
self._last_search_request = None
except Exception:
import traceback
rospy.logwarn("Error while close tab %s: %s", str(tab_index), traceback.format_exc(1))
self.upperButton.setEnabled(self.tabWidget.count() > 1)
def reject(self):
if self.find_dialog.isVisible():
self.searchButton.setChecked(not self.searchButton.isChecked())
else:
self.close()
def on_changed_file(self, grpc_path, mtime):
if grpc_path in self.files:
for i in range(self.tabWidget.count()):
if self.tabWidget.widget(i).filename == grpc_path:
self.tabWidget.widget(i).file_changed(mtime)
break
if self._last_search_request is not None:
self.on_load_request(*self._last_search_request)
def closeEvent(self, event):
'''
Test the open files for changes and save this if needed.
'''
changed = []
# get the names of all changed files
for i in range(self.tabWidget.count()):
w = self.tabWidget.widget(i)
if w.document().isModified():
changed.append(self.__getTabName(w.filename))
if changed:
# ask the user for save changes
if self.isHidden():
buttons = MessageBox.Yes | MessageBox.No
else:
buttons = MessageBox.Yes | MessageBox.No | MessageBox.Cancel
result = MessageBox.question(self, "Unsaved Changes", '\n\n'.join(["Save the file before closing?", '\n'.join(changed)]), buttons=buttons)
if result == MessageBox.Yes:
for i in range(self.tabWidget.count()):
w = self.tabWidget.widget(i).save()
self.graph_view.clear_cache()
event.accept()
elif result == MessageBox.No:
event.accept()
elif rospy.is_shutdown():
event.ignore()
else:
event.accept()
if event.isAccepted():
self.storeSetting()
nm.nmd().file.changed_file.disconnect(self.on_changed_file)
nm.nmd().file.packages_available.connect(self._on_new_packages)
self.finished_signal.emit(self.init_filenames)
def on_editor_modificationChanged(self, value=None):
'''
If the content was changed, a '*' will be shown in the tab name.
'''
tab_name = self.__getTabName(self.tabWidget.currentWidget().filename)
if (self.tabWidget.currentWidget().document().isModified()):
tab_name = '*%s' % tab_name
self.tabWidget.setTabText(self.tabWidget.currentIndex(), tab_name)
def on_editor_positionChanged(self):
'''
Shows the number of the line and column in a label.
'''
cursor = self.tabWidget.currentWidget().textCursor()
self.pos_label.setText(':%s:%s #%s' % (cursor.blockNumber() + 1, cursor.columnNumber(), cursor.position()))
def __getTabName(self, lfile):
base = os.path.basename(lfile).replace('.launch', '')
(package, _) = package_name(os.path.dirname(lfile))
return '%s [%s]' % (base, package)
def _on_new_packages(self, url):
try:
if nmdurl.nmduri_from_path(url) == nmdurl.nmduri_from_path(self.tabWidget.currentWidget().filename):
rospy.logdebug("packages updated, rebuild graph")
if self.graph_view.has_none_packages:
self.graph_view.clear_cache()
except Exception:
import traceback
print(traceback.format_exc())
##############################################################################
# HANDLER for buttons
##############################################################################
def on_clear_log_button_clicked(self):
self._log_warning_count = 0
self.show_log_button.setIcon(self._empty_icon)
self.log_browser.clear()
self.log_dock.setVisible(False)
self.show_log_button.setChecked(False)
self.tabWidget.currentWidget().setFocus()
def on_upperButton_clicked(self):
'''
Opens the file which include the current open file
'''
if self.tabWidget.currentIndex() != 0:
self.graph_view.find_parent_file()
def on_downButton_clicked(self):
'''
Select editor right from current.
'''
if self.tabWidget.currentIndex() < self.tabWidget.count():
self.tabWidget.setCurrentIndex(self.tabWidget.currentIndex() + 1)
def on_saveButton_clicked(self):
'''
Saves the current document. This method is called if the C{save button}
was clicked.
'''
saved, errors, msg = self.tabWidget.currentWidget().save()
if errors:
if msg:
rospy.logwarn(msg)
MessageBox.critical(self, "Error", "Error while save file: %s" % os.path.basename(self.tabWidget.currentWidget().filename), detailed_text=msg)
self.tabWidget.setTabIcon(self.tabWidget.currentIndex(), self._error_icon)
self.tabWidget.setTabToolTip(self.tabWidget.currentIndex(), msg)
self.on_graph_info("saved failed %s: %s" % (self.tabWidget.currentWidget().filename, msg), True)
elif saved:
self.on_graph_info("saved %s" % self.tabWidget.currentWidget().filename)
self.tabWidget.setTabIcon(self.tabWidget.currentIndex(), self._empty_icon)
self.tabWidget.setTabToolTip(self.tabWidget.currentIndex(), '')
self.graph_view.clear_cache()
def on_shortcut_find(self):
pass
def on_toggled_log(self, value):
'''
Shows the log bar
'''
if value:
self.log_dock.setVisible(True)
else:
self.log_dock.setVisible(False)
self.tabWidget.currentWidget().setFocus()
def on_toggled_graph(self, value):
'''
Shows the search frame
'''
if value:
self.graph_view.enable()
else:
# self.replaceButton.setChecked(False)
self.graph_view.setVisible(False)
self.tabWidget.currentWidget().setFocus()
def on_toggled_find(self, value, only_results=False):
'''
Shows the search frame
'''
if value:
self.find_dialog.enable()
self.find_dialog.find_frame.setVisible(not only_results)
self.find_dialog.recursive_search_box.setVisible(not only_results)
if not only_results:
# clear results if not search text exists and we show not only search results
if not self.find_dialog.search_field.text():
self.find_dialog.found_files_list.clear()
else:
self.replaceButton.setChecked(False)
self.find_dialog.setVisible(False)
self.tabWidget.currentWidget().setFocus()
def on_toggled_replace(self, value):
'''
Shows the replace lineedit in the search frame
'''
if value:
self.searchButton.setChecked(True)
self.find_dialog.set_replace_visible(value)
def on_shortcut_goto(self):
'''
Opens a C{goto} dialog.
'''
value = 1
ok = False
try:
value, ok = QInputDialog.getInt(self, "Goto", self.tr("Line number:"),
QLineEdit.Normal, minValue=1, step=1)
except Exception:
value, ok = QInputDialog.getInt(self, "Goto", self.tr("Line number:"),
QLineEdit.Normal, min=1, step=1)
if ok:
self._goto(value)
self.tabWidget.currentWidget().setFocus(Qt.ActiveWindowFocusReason)
def _goto(self, linenr, select_line=True):
if linenr > self.tabWidget.currentWidget().document().blockCount():
linenr = self.tabWidget.currentWidget().document().blockCount()
curpos = self.tabWidget.currentWidget().textCursor().blockNumber() + 1
while curpos != linenr:
mov = QTextCursor.NextBlock if curpos < linenr else QTextCursor.PreviousBlock
self.tabWidget.currentWidget().moveCursor(mov)
curpos = self.tabWidget.currentWidget().textCursor().blockNumber() + 1
self.tabWidget.currentWidget().moveCursor(QTextCursor.EndOfBlock)
self.tabWidget.currentWidget().moveCursor(QTextCursor.StartOfBlock, QTextCursor.KeepAnchor)
##############################################################################
# SLOTS for search dialog
##############################################################################
def on_search_result(self, search_text, found, path, startpos, endpos, linenr=-1, line_text=''):
'''
A slot to handle a found text. It goes to the position in the text and select
the searched text. On new file it will be open.
:param search_text: the searched text
:type search_text: str
:param found: the text was found or not
:type found: bool
:param path: the path of the file the text was found
:type path: str
:param startpos: the position in the text
:type startpos: int
:param endpos: the end position in the text
:type endpos: int
'''
if found:
if self.tabWidget.currentWidget().filename != path:
focus_widget = QApplication.focusWidget()
self.on_load_request(path)
focus_widget.setFocus()
self.tabWidget.currentWidget().select(startpos, endpos, False)
def on_search_result_on_open(self, search_text, found, path, startpos, endpos, linenr, line_text):
'''
Like on_search_result, but skips the text in comments.
'''
if found:
self._search_node_count += 1
if self._search_node_count > 1:
self.on_toggled_find(True, only_results=True)
self.find_dialog.current_search_text = search_text
self.find_dialog.on_search_result(search_text, found, path, startpos, endpos, linenr, line_text)
self.on_graph_info("search thread: found %s in '%s:%d'" % (search_text, path, linenr))
if self.tabWidget.currentWidget().filename != path:
focus_widget = QApplication.focusWidget()
self.on_load_request(path)
if focus_widget is not None:
focus_widget.setFocus()
self.tabWidget.currentWidget().select(startpos, endpos, True)
# self.on_search_result(search_text, found, path, startpos, endpos, linenr, line_text)
def on_search_result_warning(self, msg):
self.on_graph_info("search thread: %s" % (msg), True)
def on_replace(self, search_text, path, index, replaced_text):
'''
A slot to handle a text replacement of the TextSearchFrame.
:param search_text: the searched text
:type search_text: str
:param path: the path of the file the text was found
:type path: str
:param index: the position in the text
:type index: int
:param replaced_text: the new text
:type replaced_text: str
'''
cursor = self.tabWidget.currentWidget().textCursor()
if cursor.selectedText() == search_text:
cursor.insertText(replaced_text)
##############################################################################
# LAUNCH TAG insertion
##############################################################################
def _show_custom_parameter_dialog(self):
methods = {'nm/associations': self._on_add_cp_associations,
'capability_group': self._on_add_cp_capability_group,
'nm/kill_on_stop': self._on_add_cp_kill_on_stop,
'autostart/delay': self._on_add_cp_as_delay,
'autostart/exclude': self._on_add_cp_as_exclude,
'autostart/required_publisher': self._on_add_cp_as_req_publisher,
'respawn/max': self._on_add_cp_r_max,
'respawn/min_runtime': self._on_add_cp_r_min_runtime,
'respawn/delay': self._on_add_cp_r_delay,
}
res = SelectDialog.getValue('Insert custom parameter', "Select parameter to insert:",
sorted(methods.keys()), exclusive=True, parent=self,
select_if_single=False,
store_geometry='insert_param')
tags2insert = res[0]
for tag in tags2insert:
methods[tag]()
def _create_tag_button(self, parent=None):
btn = QPushButton(parent)
btn.setObjectName("tagButton")
btn.setText(self._translate("Add &tag"))
# btn.setShortcut("Ctrl+T")
btn.setToolTip('Adds a ROS launch tag to launch file')
btn.setMenu(self._create_tag_menu(btn))
btn.setFlat(True)
return btn
def _create_tag_menu(self, parent=None):
# creates a tag menu
tag_menu = QMenu("ROS Tags", parent)
# group tag
add_group_tag_action = QAction("<group>", self, statusTip="", triggered=self._on_add_group_tag)
add_group_tag_action.setShortcuts(QKeySequence("Ctrl+Shift+g"))
tag_menu.addAction(add_group_tag_action)
# node tag
add_node_tag_action = QAction("<node>", self, statusTip="", triggered=self._on_add_node_tag)
add_node_tag_action.setShortcuts(QKeySequence("Ctrl+Shift+n"))
tag_menu.addAction(add_node_tag_action)
# node tag with all attributes
add_node_tag_all_action = QAction("<node all>", self, statusTip="", triggered=self._on_add_node_tag_all)
tag_menu.addAction(add_node_tag_all_action)
# include tag with all attributes
add_include_tag_all_action = QAction("<include>", self, statusTip="", triggered=self._on_add_include_tag_all)
add_include_tag_all_action.setShortcuts(QKeySequence("Ctrl+Shift+i"))
tag_menu.addAction(add_include_tag_all_action)
# remap
add_remap_tag_action = QAction("<remap>", self, statusTip="", triggered=self._on_add_remap_tag)
add_remap_tag_action.setShortcuts(QKeySequence("Ctrl+Shift+r"))
tag_menu.addAction(add_remap_tag_action)
# env tag
add_env_tag_action = QAction("<env>", self, statusTip="", triggered=self._on_add_env_tag)
tag_menu.addAction(add_env_tag_action)
# param tag
add_param_clipboard_tag_action = QAction("<param value>", self, statusTip="add value from clipboard", triggered=self._on_add_param_clipboard_tag)
add_param_clipboard_tag_action.setShortcuts(QKeySequence("Ctrl+Shift+p"))
tag_menu.addAction(add_param_clipboard_tag_action)
add_param_tag_action = QAction("<param>", self, statusTip="", triggered=self._on_add_param_tag)
add_param_tag_action.setShortcuts(QKeySequence("Ctrl+Shift+Alt+p"))
tag_menu.addAction(add_param_tag_action)
# param tag with all attributes
add_param_tag_all_action = QAction("<param all>", self, statusTip="", triggered=self._on_add_param_tag_all)
tag_menu.addAction(add_param_tag_all_action)
# rosparam tag with all attributes
add_rosparam_tag_all_action = QAction("<rosparam>", self, statusTip="", triggered=self._on_add_rosparam_tag_all)
tag_menu.addAction(add_rosparam_tag_all_action)
# arg tag with default definition
add_arg_tag_default_action = QAction("<arg default>", self, statusTip="", triggered=self._on_add_arg_tag_default)
add_arg_tag_default_action.setShortcuts(QKeySequence("Ctrl+Shift+a"))
tag_menu.addAction(add_arg_tag_default_action)
# arg tag with value definition
add_arg_tag_value_action = QAction("<arg value>", self, statusTip="", triggered=self._on_add_arg_tag_value)
add_arg_tag_value_action.setShortcuts(QKeySequence("Ctrl+Alt+a"))
tag_menu.addAction(add_arg_tag_value_action)
# test tag
add_test_tag_action = QAction("<test>", self, statusTip="", triggered=self._on_add_test_tag)
add_test_tag_action.setShortcuts(QKeySequence("Ctrl+Alt+t"))
tag_menu.addAction(add_test_tag_action)
# test tag with all attributes
add_test_tag_all_action = QAction("<test all>", self, statusTip="", triggered=self._on_add_test_tag_all)
tag_menu.addAction(add_test_tag_all_action)
sub_cp_menu = QMenu("Custom parameters", parent)
show_cp_dialog_action = QAction("Show Dialog", self, statusTip="", triggered=self._show_custom_parameter_dialog)
show_cp_dialog_action.setShortcuts(QKeySequence("Ctrl+Shift+d"))
sub_cp_menu.addAction(show_cp_dialog_action)
add_cp_associations_action = QAction("nm/associations", self, statusTip="", triggered=self._on_add_cp_associations)
add_cp_associations_action.setShortcuts(QKeySequence("Ctrl+Alt+a"))
sub_cp_menu.addAction(add_cp_associations_action)
sub_cp_as_menu = QMenu("Autostart", parent)
add_cp_as_delay_action = QAction("delay", self, statusTip="", triggered=self._on_add_cp_as_delay)
sub_cp_as_menu.addAction(add_cp_as_delay_action)
add_cp_as_exclude_action = QAction("exclude", self, statusTip="", triggered=self._on_add_cp_as_exclude)
sub_cp_as_menu.addAction(add_cp_as_exclude_action)
add_cp_as_req_publisher_action = QAction("required publisher", self, statusTip="", triggered=self._on_add_cp_as_req_publisher)
sub_cp_as_menu.addAction(add_cp_as_req_publisher_action)
sub_cp_menu.addMenu(sub_cp_as_menu)
sub_cp_r_menu = QMenu("Respawn", parent)
add_cp_r_max_action = QAction("max", self, statusTip="", triggered=self._on_add_cp_r_max)
sub_cp_r_menu.addAction(add_cp_r_max_action)
add_cp_r_min_runtime_action = QAction("min_runtime", self, statusTip="", triggered=self._on_add_cp_r_min_runtime)
sub_cp_r_menu.addAction(add_cp_r_min_runtime_action)
add_cp_r_delay_action = QAction("delay", self, statusTip="", triggered=self._on_add_cp_r_delay)
sub_cp_r_menu.addAction(add_cp_r_delay_action)
sub_cp_menu.addMenu(sub_cp_r_menu)
add_cp_capability_group_action = QAction("capability_group", self, statusTip="", triggered=self._on_add_cp_capability_group)
add_cp_capability_group_action.setShortcuts(QKeySequence("Ctrl+Alt+p"))
sub_cp_menu.addAction(add_cp_capability_group_action)
add_cp_kill_on_stop_action = QAction("nm/kill_on_stop", self, statusTip="True or time to wait in ms", triggered=self._on_add_cp_kill_on_stop)
add_cp_kill_on_stop_action.setShortcuts(QKeySequence("Ctrl+Shift+k"))
sub_cp_menu.addAction(add_cp_kill_on_stop_action)
tag_menu.addMenu(sub_cp_menu)
return tag_menu
def _insert_text(self, text, cursor_pose=None, selection_len=None):
if self.tabWidget.currentWidget().isReadOnly():
return
cursor = self.tabWidget.currentWidget().textCursor()
if not cursor.isNull():
cursor.beginEditBlock()
col = cursor.columnNumber()
spaces = ''.join([' ' for _ in range(col)])
curr_cursor_pos = cursor.position()
cursor.insertText(text.replace('\n', '\n%s' % spaces))
if cursor_pose is not None:
cursor.setPosition(curr_cursor_pos + cursor_pose, QTextCursor.MoveAnchor)
if selection_len is not None:
cursor.movePosition(QTextCursor.NextCharacter, QTextCursor.KeepAnchor, selection_len)
cursor.endEditBlock()
self.tabWidget.currentWidget().setTextCursor(cursor)
self.tabWidget.currentWidget().setFocus(Qt.OtherFocusReason)
def _on_add_group_tag(self):
self._insert_text('<group ns="namespace" clear_params="true|false">\n'
'</group>', 11, 9)
def _get_package_dialog(self):
muri = masteruri_from_ros()
if self.init_filenames:
muri = nmdurl.masteruri(self.init_filenames[0])
return PackageDialog(muri)
def _on_add_node_tag(self):
dia = self._get_package_dialog()
if dia.exec_():
self._insert_text('<node name="%s" pkg="%s" type="%s">\n'
'</node>' % (dia.binary, dia.package, dia.binary))
def _on_add_node_tag_all(self):
dia = self._get_package_dialog()
if dia.exec_():
self._insert_text('<node name="%s" pkg="%s" type="%s"\n'
' args="arg1" machine="machine_name"\n'
' respawn="true" required="true"\n'
' ns="foo" clear_params="true|false"\n'
' output="log|screen" cwd="ROS_HOME|node"\n'
' launch-prefix="prefix arguments">\n'
'</node>' % (dia.binary, dia.package, dia.binary))
def _on_add_include_tag_all(self):
self._insert_text('<include file="$(find pkg-name)/path/filename.xml"\n'
' ns="foo" clear_params="true|false">\n'
'</include>', 22, 27)
def _on_add_remap_tag(self):
self._insert_text('<remap from="original" to="new"/>', 13, 8)
def _on_add_env_tag(self):
self._insert_text('<env name="variable" value="value"/>', 11, 8)
def _on_add_param_clipboard_tag(self):
lines = QApplication.clipboard().mimeData().text().splitlines()
name = ""
if len(lines) == 1:
name = lines[0]
self._insert_text('<param name="%s" value="value" />' % name, 22 + len(name), 5)
def _on_add_param_tag(self):
self._insert_text('<param name="name" value="value" />', 13, 4)
def _on_add_param_tag_all(self):
self._insert_text('<param name="name" value="value"\n'
' type="str|int|double|bool"\n'
' textfile="$(find pkg-name)/path/file.txt"\n'
' binfile="$(find pkg-name)/path/file"\n'
' command="$(find pkg-name)/exe \'$(find pkg-name)/arg.txt\'">\n'
'</param>', 13, 4)
def _on_add_rosparam_tag_all(self):
self._insert_text('<rosparam param="name"\n'
' file="$(find pkg-name)/path/foo.yaml"\n'
' command="load|dump|delete"\n'
' ns="namespace"\n'
' subst_value="true|false">\n'
'</rosparam>', 17, 4)
def _on_add_arg_tag_default(self):
self._insert_text('<arg name="foo" default="1" />', 11, 3)
def _on_add_arg_tag_value(self):
self._insert_text('<arg name="foo" value="bar" />', 11, 3)
def _on_add_test_tag(self):
dia = self._get_package_dialog()
if dia.exec_():
self._insert_text('<test name="%s" pkg="%s" type="%s" test-name="test_%s">\n'
'</test>' % (dia.binary, dia.package, dia.binary, dia.binary))
def _on_add_test_tag_all(self):
dia = self._get_package_dialog()
if dia.exec_():
self._insert_text('<test name="%s" pkg="%s" type="%s" test-name="test_%s">\n'
' args="arg1" time-limit="60.0"\n'
' ns="foo" clear_params="true|false"\n'
' cwd="ROS_HOME|node" retry="0"\n'
' launch-prefix="prefix arguments">\n'
'</test>' % (dia.binary, dia.package, dia.binary, dia.binary))
def _on_add_cp_capability_group(self):
self._insert_text('<param name="capability_group" value="demo" />', 38, 4)
def _on_add_cp_kill_on_stop(self):
self._insert_text('<param name="nm/kill_on_stop" value="100" hint="[ms]" />', 34, 3)
def _on_add_cp_associations(self):
self._insert_text('<param name="nm/associations" value="node1,node2" hint="list of nodes" />', 34, 11)
def _on_add_cp_as_delay(self):
self._insert_text('<param name="autostart/delay" value="1" hint="[seconds]" />', 37, 1)
def _on_add_cp_as_exclude(self):
self._insert_text('<param name="autostart/exclude" value="True" />', 39, 4)
def _on_add_cp_as_req_publisher(self):
self._insert_text('<param name="autostart/required/publisher" value="topic" />', 50, 5)
def _on_add_cp_r_max(self):
self._insert_text('<param name="respawn/max" value="10" />', 33, 2)
def _on_add_cp_r_min_runtime(self):
self._insert_text('<param name="respawn/min_runtime" value="10" hint="[seconds]" />', 41, 2)
def _on_add_cp_r_delay(self):
self._insert_text('<param name="respawn/delay" value="5" hint="[seconds]" />', 31, 2)
def __init__(self, context):
# super(BDIPelvisPoseWidget, self).__init__(context)
# self.setObjectName('BDIPelvisPoseWidget')
super(BDIPelvisPoseWidget, self).__init__()
self.name = "BDIPelvisPoseWidget"
self.updateStateSignal.connect(self.on_updateState)
# self._widget = QWidget()
self._widget = context
vbox = QVBoxLayout()
self.forward_position = 0.0
self.lateral_position = 0.0
self.height_position = 0.91
self.roll_position = 0.0
self.pitch_position = 0.0
self.yaw_position = 0.0
self.currentForward = 0.0
self.currentLateral = 0.0
self.currentHeight = 0.91
self.currentRoll = 0.0
self.currentPitch = 0.0
self.currentYaw = 0.0
# Define checkboxes
vbox = QVBoxLayout()
label = QLabel()
label.setText("BDI Pelvis Height (Manipulate Mode Only)") # todo - connect controller mode
vbox.addWidget(label)
self.enable_checkbox = QCheckBox("Enable")
self.enable_checkbox.stateChanged.connect(self.on_enable_check)
vbox.addWidget(self.enable_checkbox)
self.snap_to_current_button = QPushButton("Snap to Current")
self.snap_to_current_button.pressed.connect(self.on_snapCurrentPressed)
vbox.addWidget(self.snap_to_current_button)
self.roll_slider = QSlider(Qt.Horizontal)
self.roll_label = QLabel()
self.roll_label.setText("Roll")
vbox.addWidget(self.roll_label)
self.roll_slider.setRange(int(-100), int(101))
self.roll_slider.setValue(int(0))
self.roll_slider.setSingleStep((200) / 50)
self.roll_slider.setTickInterval(25)
self.roll_slider.valueChanged.connect(self.on_rollSliderMoved)
vbox.addWidget(self.roll_slider)
self.roll_progress_bar = QProgressBar()
self.roll_progress_bar.setRange(int(-100), int(101))
self.roll_progress_bar.setValue((6.0 / math.pi) * self.currentRoll * 100)
self.roll_progress_bar.setFormat("%.6f" % self.currentRoll)
vbox.addWidget(self.roll_progress_bar)
self.pitch_slider = QSlider(Qt.Horizontal)
self.pitch_label = QLabel()
self.pitch_label.setText("Pitch")
vbox.addWidget(self.pitch_label)
self.pitch_slider.setRange(int(-100), int(101))
self.pitch_slider.setValue(int(0))
self.pitch_slider.setSingleStep((200) / 50)
self.pitch_slider.setTickInterval(25)
self.pitch_slider.valueChanged.connect(self.on_pitchSliderMoved)
vbox.addWidget(self.pitch_slider)
self.pitch_progress_bar = QProgressBar()
self.pitch_progress_bar.setRange(int(-100), int(101))
self.pitch_progress_bar.setValue((6.0 / math.pi) * self.currentPitch * 100)
self.pitch_progress_bar.setFormat("%.6f" % self.currentPitch)
vbox.addWidget(self.pitch_progress_bar)
self.yaw_slider = QSlider(Qt.Horizontal)
self.yaw_label = QLabel()
self.yaw_label.setText("Yaw")
vbox.addWidget(self.yaw_label)
self.yaw_slider.setRange(int(-100), int(101))
self.yaw_slider.setValue(int(0))
self.yaw_slider.setSingleStep((200) / 50)
self.yaw_slider.setTickInterval(25)
self.yaw_slider.valueChanged.connect(self.on_yawSliderMoved)
vbox.addWidget(self.yaw_slider)
self.yaw_progress_bar = QProgressBar()
self.yaw_progress_bar.setRange(int(-100), int(101))
self.yaw_progress_bar.setValue((4.0 / math.pi) * self.currentYaw * 100)
self.yaw_progress_bar.setFormat("%.6f" % self.currentYaw)
vbox.addWidget(self.yaw_progress_bar)
self.forward_label = QLabel()
self.forward_label.setText("Forward")
vbox.addWidget(self.forward_label)
widget = QWidget()
hbox = QHBoxLayout()
hbox.addStretch()
self.forward_slider = QSlider(Qt.Vertical)
# self.forward_slider.setText("Height")
self.forward_slider.setRange(int(-101), int(100))
self.forward_slider.setValue(int(0))
self.forward_slider.setSingleStep(1)
self.forward_slider.setTickInterval(10)
self.forward_slider.valueChanged.connect(self.on_forwardSliderMoved)
hbox.addWidget(self.forward_slider)
self.forward_progress_bar = QProgressBar()
self.forward_progress_bar.setOrientation(Qt.Vertical)
self.forward_progress_bar.setRange(int(-100), int(101))
self.forward_progress_bar.setValue((self.currentForward / 0.075) * 100)
self.forward_progress_bar.setTextVisible(False)
hbox.addWidget(self.forward_progress_bar)
self.forward_progress_bar_label = QLabel()
self.forward_progress_bar_label.setText("%.6f" % self.currentForward)
hbox.addWidget(self.forward_progress_bar_label)
hbox.addStretch()
widget.setLayout(hbox)
vbox.addWidget(widget)
self.lateral_label = QLabel()
self.lateral_label.setText("Lateral")
vbox.addWidget(self.lateral_label)
self.lateral_slider = QSlider(Qt.Horizontal)
# self.lateral_slider.setText("Lateral")
self.lateral_slider.setRange(int(-100), int(101))
self.lateral_slider.setValue(int(0))
self.lateral_slider.setSingleStep((200) / 50)
self.lateral_slider.setTickInterval(25)
self.lateral_slider.valueChanged.connect(self.on_lateralSliderMoved)
vbox.addWidget(self.lateral_slider)
self.lateral_progress_bar = QProgressBar()
self.lateral_progress_bar.setRange(int(-100), int(101))
self.lateral_progress_bar.setValue((self.currentLateral / 0.15) * 100)
self.lateral_progress_bar.setFormat("%.6f" % self.currentLateral)
vbox.addWidget(self.lateral_progress_bar)
self.height_label = QLabel()
self.height_label.setText("Height")
vbox.addWidget(self.height_label)
widget = QWidget()
hbox = QHBoxLayout()
hbox.addStretch()
self.height_slider = QSlider(Qt.Vertical)
# self.height_slider.setText("Height")
self.height_slider.setRange(int(55), int(105))
self.height_slider.setValue(int(91))
self.height_slider.setSingleStep(2)
self.height_slider.setTickInterval(10)
self.height_slider.valueChanged.connect(self.on_heightSliderMoved)
hbox.addWidget(self.height_slider)
self.height_progress_bar = QProgressBar()
self.height_progress_bar.setOrientation(Qt.Vertical)
self.height_progress_bar.setRange(int(55), int(105))
self.height_progress_bar.setValue(self.currentHeight * 100)
self.height_progress_bar.setTextVisible(False)
hbox.addWidget(self.height_progress_bar)
self.height_progress_bar_label = QLabel()
self.height_progress_bar_label.setText("%.6f" % self.currentHeight)
hbox.addWidget(self.height_progress_bar_label)
hbox.addStretch()
widget.setLayout(hbox)
vbox.addWidget(widget)
vbox.addStretch()
self._widget.setLayout(vbox)
# context.add_widget(self._widget)
self.height_position = 0.91
self.lateral_position = 0.0
self.yaw_position = 0.0
self.first_time = True
self.enable_checkbox.setChecked(False)
self.yaw_slider.setEnabled(False)
self.roll_slider.setEnabled(False)
self.pitch_slider.setEnabled(False)
self.forward_slider.setEnabled(False)
self.lateral_slider.setEnabled(False)
self.height_slider.setEnabled(False)
self.pub_robot = rospy.Publisher("/flor/controller/bdi_desired_pelvis_pose", PoseStamped, queue_size=10)
# self.stateSubscriber = rospy.Subscriber('/flor/pelvis_controller/current_states',JointState, self.stateCallbackFnc)
# self.pub_bdi_pelvis = rospy.Publisher('/bdi_manipulate_pelvis_pose_rpy',PoseStamped)
self.pelvis_trajectory_pub = rospy.Publisher(
"/robot_controllers/pelvis_traj_controller/command", JointTrajectory, queue_size=10
)
self.stateSubscriber = rospy.Subscriber(
"/robot_controllers/pelvis_traj_controller/state", JointTrajectoryControllerState, self.stateCallbackFnc
)
class BDIPelvisPoseWidget(QObject):
updateStateSignal = Signal(object)
def __init__(self, context):
# super(BDIPelvisPoseWidget, self).__init__(context)
# self.setObjectName('BDIPelvisPoseWidget')
super(BDIPelvisPoseWidget, self).__init__()
self.name = "BDIPelvisPoseWidget"
self.updateStateSignal.connect(self.on_updateState)
# self._widget = QWidget()
self._widget = context
vbox = QVBoxLayout()
self.forward_position = 0.0
self.lateral_position = 0.0
self.height_position = 0.91
self.roll_position = 0.0
self.pitch_position = 0.0
self.yaw_position = 0.0
self.currentForward = 0.0
self.currentLateral = 0.0
self.currentHeight = 0.91
self.currentRoll = 0.0
self.currentPitch = 0.0
self.currentYaw = 0.0
# Define checkboxes
vbox = QVBoxLayout()
label = QLabel()
label.setText("BDI Pelvis Height (Manipulate Mode Only)") # todo - connect controller mode
vbox.addWidget(label)
self.enable_checkbox = QCheckBox("Enable")
self.enable_checkbox.stateChanged.connect(self.on_enable_check)
vbox.addWidget(self.enable_checkbox)
self.snap_to_current_button = QPushButton("Snap to Current")
self.snap_to_current_button.pressed.connect(self.on_snapCurrentPressed)
vbox.addWidget(self.snap_to_current_button)
self.roll_slider = QSlider(Qt.Horizontal)
self.roll_label = QLabel()
self.roll_label.setText("Roll")
vbox.addWidget(self.roll_label)
self.roll_slider.setRange(int(-100), int(101))
self.roll_slider.setValue(int(0))
self.roll_slider.setSingleStep((200) / 50)
self.roll_slider.setTickInterval(25)
self.roll_slider.valueChanged.connect(self.on_rollSliderMoved)
vbox.addWidget(self.roll_slider)
self.roll_progress_bar = QProgressBar()
self.roll_progress_bar.setRange(int(-100), int(101))
self.roll_progress_bar.setValue((6.0 / math.pi) * self.currentRoll * 100)
self.roll_progress_bar.setFormat("%.6f" % self.currentRoll)
vbox.addWidget(self.roll_progress_bar)
self.pitch_slider = QSlider(Qt.Horizontal)
self.pitch_label = QLabel()
self.pitch_label.setText("Pitch")
vbox.addWidget(self.pitch_label)
self.pitch_slider.setRange(int(-100), int(101))
self.pitch_slider.setValue(int(0))
self.pitch_slider.setSingleStep((200) / 50)
self.pitch_slider.setTickInterval(25)
self.pitch_slider.valueChanged.connect(self.on_pitchSliderMoved)
vbox.addWidget(self.pitch_slider)
self.pitch_progress_bar = QProgressBar()
self.pitch_progress_bar.setRange(int(-100), int(101))
self.pitch_progress_bar.setValue((6.0 / math.pi) * self.currentPitch * 100)
self.pitch_progress_bar.setFormat("%.6f" % self.currentPitch)
vbox.addWidget(self.pitch_progress_bar)
self.yaw_slider = QSlider(Qt.Horizontal)
self.yaw_label = QLabel()
self.yaw_label.setText("Yaw")
vbox.addWidget(self.yaw_label)
self.yaw_slider.setRange(int(-100), int(101))
self.yaw_slider.setValue(int(0))
self.yaw_slider.setSingleStep((200) / 50)
self.yaw_slider.setTickInterval(25)
self.yaw_slider.valueChanged.connect(self.on_yawSliderMoved)
vbox.addWidget(self.yaw_slider)
self.yaw_progress_bar = QProgressBar()
self.yaw_progress_bar.setRange(int(-100), int(101))
self.yaw_progress_bar.setValue((4.0 / math.pi) * self.currentYaw * 100)
self.yaw_progress_bar.setFormat("%.6f" % self.currentYaw)
vbox.addWidget(self.yaw_progress_bar)
self.forward_label = QLabel()
self.forward_label.setText("Forward")
vbox.addWidget(self.forward_label)
widget = QWidget()
hbox = QHBoxLayout()
hbox.addStretch()
self.forward_slider = QSlider(Qt.Vertical)
# self.forward_slider.setText("Height")
self.forward_slider.setRange(int(-101), int(100))
self.forward_slider.setValue(int(0))
self.forward_slider.setSingleStep(1)
self.forward_slider.setTickInterval(10)
self.forward_slider.valueChanged.connect(self.on_forwardSliderMoved)
hbox.addWidget(self.forward_slider)
self.forward_progress_bar = QProgressBar()
self.forward_progress_bar.setOrientation(Qt.Vertical)
self.forward_progress_bar.setRange(int(-100), int(101))
self.forward_progress_bar.setValue((self.currentForward / 0.075) * 100)
self.forward_progress_bar.setTextVisible(False)
hbox.addWidget(self.forward_progress_bar)
self.forward_progress_bar_label = QLabel()
self.forward_progress_bar_label.setText("%.6f" % self.currentForward)
hbox.addWidget(self.forward_progress_bar_label)
hbox.addStretch()
widget.setLayout(hbox)
vbox.addWidget(widget)
self.lateral_label = QLabel()
self.lateral_label.setText("Lateral")
vbox.addWidget(self.lateral_label)
self.lateral_slider = QSlider(Qt.Horizontal)
# self.lateral_slider.setText("Lateral")
self.lateral_slider.setRange(int(-100), int(101))
self.lateral_slider.setValue(int(0))
self.lateral_slider.setSingleStep((200) / 50)
self.lateral_slider.setTickInterval(25)
self.lateral_slider.valueChanged.connect(self.on_lateralSliderMoved)
vbox.addWidget(self.lateral_slider)
self.lateral_progress_bar = QProgressBar()
self.lateral_progress_bar.setRange(int(-100), int(101))
self.lateral_progress_bar.setValue((self.currentLateral / 0.15) * 100)
self.lateral_progress_bar.setFormat("%.6f" % self.currentLateral)
vbox.addWidget(self.lateral_progress_bar)
self.height_label = QLabel()
self.height_label.setText("Height")
vbox.addWidget(self.height_label)
widget = QWidget()
hbox = QHBoxLayout()
hbox.addStretch()
self.height_slider = QSlider(Qt.Vertical)
# self.height_slider.setText("Height")
self.height_slider.setRange(int(55), int(105))
self.height_slider.setValue(int(91))
self.height_slider.setSingleStep(2)
self.height_slider.setTickInterval(10)
self.height_slider.valueChanged.connect(self.on_heightSliderMoved)
hbox.addWidget(self.height_slider)
self.height_progress_bar = QProgressBar()
self.height_progress_bar.setOrientation(Qt.Vertical)
self.height_progress_bar.setRange(int(55), int(105))
self.height_progress_bar.setValue(self.currentHeight * 100)
self.height_progress_bar.setTextVisible(False)
hbox.addWidget(self.height_progress_bar)
self.height_progress_bar_label = QLabel()
self.height_progress_bar_label.setText("%.6f" % self.currentHeight)
hbox.addWidget(self.height_progress_bar_label)
hbox.addStretch()
widget.setLayout(hbox)
vbox.addWidget(widget)
vbox.addStretch()
self._widget.setLayout(vbox)
# context.add_widget(self._widget)
self.height_position = 0.91
self.lateral_position = 0.0
self.yaw_position = 0.0
self.first_time = True
self.enable_checkbox.setChecked(False)
self.yaw_slider.setEnabled(False)
self.roll_slider.setEnabled(False)
self.pitch_slider.setEnabled(False)
self.forward_slider.setEnabled(False)
self.lateral_slider.setEnabled(False)
self.height_slider.setEnabled(False)
self.pub_robot = rospy.Publisher("/flor/controller/bdi_desired_pelvis_pose", PoseStamped, queue_size=10)
# self.stateSubscriber = rospy.Subscriber('/flor/pelvis_controller/current_states',JointState, self.stateCallbackFnc)
# self.pub_bdi_pelvis = rospy.Publisher('/bdi_manipulate_pelvis_pose_rpy',PoseStamped)
self.pelvis_trajectory_pub = rospy.Publisher(
"/robot_controllers/pelvis_traj_controller/command", JointTrajectory, queue_size=10
)
self.stateSubscriber = rospy.Subscriber(
"/robot_controllers/pelvis_traj_controller/state", JointTrajectoryControllerState, self.stateCallbackFnc
)
def stateCallbackFnc(self, jointState_msg):
self.updateStateSignal.emit(jointState_msg)
def shutdown_plugin(self):
# Just make sure to stop timers and publishers, unsubscribe from Topics etc in the shutdown_plugin method.
print "Shutdown BDI pelvis height "
self.pub_robot.unregister()
self.stateSubscriber.unregister()
def publishRobotPelvisPose(self):
print "publishing new pelvis pose ..."
bdi_pelvis_pose = PoseStamped()
bdi_pelvis_pose.header.stamp = rospy.Time.now()
bdi_pelvis_pose.pose.position.x = self.forward_position
bdi_pelvis_pose.pose.position.y = self.lateral_position
bdi_pelvis_pose.pose.position.z = self.height_position
# Use BDI yaw*roll*pitch concatenation
xaxis, yaxis, zaxis = (1, 0, 0), (0, 1, 0), (0, 0, 1)
Rx = tf.transformations.rotation_matrix(self.roll_position, xaxis)
Ry = tf.transformations.rotation_matrix(self.pitch_position, yaxis)
Rz = tf.transformations.rotation_matrix(self.yaw_position, zaxis)
R = tf.transformations.concatenate_matrices(Rz, Rx, Ry)
q = tf.transformations.quaternion_from_matrix(R)
bdi_pelvis_pose.pose.orientation.w = q[3]
bdi_pelvis_pose.pose.orientation.x = q[0]
bdi_pelvis_pose.pose.orientation.y = q[1]
bdi_pelvis_pose.pose.orientation.z = q[2]
# w = math.cos(self.yaw_position*0.5)
# bdi_pelvis_pose.pose.orientation.x = 0.0
# bdi_pelvis_pose.pose.orientation.y = 0.0
# bdi_pelvis_pose.pose.orientation.z = math.sin(self.yaw_position*0.5)
print bdi_pelvis_pose
print q
euler = tf.transformations.euler_from_quaternion(q)
print euler
self.pub_robot.publish(bdi_pelvis_pose)
# Now publish the trajectory form for the new controllers
trajectory = JointTrajectory()
trajectory.header.stamp = rospy.Time.now()
trajectory.joint_names = ["com_v1", "com_v0", "pelvis_height", "pelvis_roll", "pelvis_pitch", "pelvis_yaw"]
trajectory.points = [JointTrajectoryPoint()]
trajectory.points[0].positions = [
self.lateral_position,
self.forward_position,
self.height_position,
self.roll_position,
self.pitch_position,
self.yaw_position,
]
trajectory.points[0].velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
trajectory.points[0].time_from_start = rospy.Duration(0.75)
self.pelvis_trajectory_pub.publish(trajectory)
###################################
def on_snapCurrentPressed(self):
print "Snap ", self.name, " values to current pelvis positions"
self.blockSignals(True)
self.resetCurrentPelvisSliders()
self.blockSignals(False)
def blockSignals(self, block):
self.yaw_slider.blockSignals(block)
self.roll_slider.blockSignals(block)
self.pitch_slider.blockSignals(block)
self.forward_slider.blockSignals(block)
self.lateral_slider.blockSignals(block)
self.height_slider.blockSignals(block)
def resetCurrentPelvisSliders(self):
self.yaw_slider.setValue((4.0 / math.pi) * self.currentYaw * 100)
self.roll_slider.setValue((6.0 / math.pi) * self.currentRoll * 100)
self.pitch_slider.setValue((6.0 / math.pi) * self.currentPitch * 100)
self.forward_slider.setValue((self.currentForward / 0.075) * 100)
self.lateral_slider.setValue((self.currentLateral / 0.15) * 100)
self.height_slider.setValue(self.currentHeight * 100)
self.yaw_label.setText("Yaw: " + str(self.currentYaw))
self.roll_label.setText("Roll: " + str(self.currentRoll))
self.pitch_label.setText("Pitch: " + str(self.currentPitch))
self.forward_label.setText("Forward: " + str(self.currentForward))
self.lateral_label.setText("Lateral: " + str(self.currentLateral))
self.height_label.setText("Height: " + str(self.currentHeight))
self.forward_position = self.currentForward
self.lateral_position = self.currentLateral
self.height_position = self.currentHeight
self.roll_position = self.currentRoll
self.pitch_position = self.currentPitch
self.yaw_position = self.currentYaw
def on_updateState(self, jointState_msg):
self.currentLateral = jointState_msg.actual.positions[0]
self.currentForward = jointState_msg.actual.positions[1]
self.currentHeight = jointState_msg.actual.positions[2]
self.currentRoll = jointState_msg.actual.positions[3]
self.currentPitch = jointState_msg.actual.positions[4]
self.currentYaw = jointState_msg.actual.positions[5]
self.yaw_progress_bar.setValue((4.0 / math.pi) * self.currentYaw * 100)
self.yaw_progress_bar.setFormat("%.6f" % self.currentYaw)
self.roll_progress_bar.setValue((6.0 / math.pi) * self.currentRoll * 100)
self.roll_progress_bar.setFormat("%.6f" % self.currentRoll)
self.pitch_progress_bar.setValue((6.0 / math.pi) * self.currentPitch * 100)
self.pitch_progress_bar.setFormat("%.6f" % self.currentPitch)
self.forward_progress_bar.setValue((self.currentForward / 0.075) * 100)
self.forward_progress_bar_label.setText("%.6f" % self.currentForward)
self.lateral_progress_bar.setValue((self.currentLateral / 0.15) * 100)
self.lateral_progress_bar.setFormat("%.6f" % self.currentYaw)
self.height_progress_bar.setValue(self.currentHeight * 100)
self.height_progress_bar_label.setText("%.6f" % self.currentHeight)
####################################
def on_enable_check(self, value):
print "Toggle the enabling checkbox - current state is ", value
self.yaw_slider.setEnabled(self.enable_checkbox.isChecked())
self.roll_slider.setEnabled(self.enable_checkbox.isChecked())
self.pitch_slider.setEnabled(self.enable_checkbox.isChecked())
self.forward_slider.setEnabled(self.enable_checkbox.isChecked())
self.lateral_slider.setEnabled(self.enable_checkbox.isChecked())
self.height_slider.setEnabled(self.enable_checkbox.isChecked())
def on_yawSliderMoved(self, value):
self.yaw_position = (value / 100.0) * math.pi / 4.0
# print "New yaw=",self.yaw_position
self.yaw_label.setText("Yaw: " + str(self.yaw_position))
if self.enable_checkbox.isChecked():
self.publishRobotPelvisPose()
def on_rollSliderMoved(self, value):
self.roll_position = (value / 100.0) * math.pi / 6.0
self.roll_label.setText("Roll: " + str(self.roll_position))
if self.enable_checkbox.isChecked():
self.publishRobotPelvisPose()
def on_pitchSliderMoved(self, value):
self.pitch_position = (value / 100.0) * math.pi / 6.0
self.pitch_label.setText("Pitch: " + str(self.pitch_position))
if self.enable_checkbox.isChecked():
self.publishRobotPelvisPose()
def on_forwardSliderMoved(self, value):
self.forward_position = (value / 100.0) * 0.075
# print "New forward=",self.forward_position
self.forward_label.setText("Forward: " + str(self.forward_position))
if self.enable_checkbox.isChecked():
self.publishRobotPelvisPose()
def on_lateralSliderMoved(self, value):
self.lateral_position = (value / 100.0) * 0.15
# print "New lateral=",self.lateral_position
self.lateral_label.setText("Lateral: " + str(self.lateral_position))
if self.enable_checkbox.isChecked():
self.publishRobotPelvisPose()
def on_heightSliderMoved(self, value):
self.height_position = value / 100.0
# print "New height=",self.height_position
self.height_label.setText("Height: " + str(self.height_position))
if self.enable_checkbox.isChecked():
self.publishRobotPelvisPose()
class Editor(QMainWindow):
'''
Creates a dialog to edit a launch file.
'''
finished_signal = Signal(list)
'''
finished_signal has as parameter the filenames of the initialization and is emitted, if this
dialog was closed.
'''
def __init__(self, filenames, search_text='', parent=None):
'''
@param filenames: a list with filenames. The last one will be activated.
@type filenames: C{[str, ...]}
@param search_text: if not empty, searches in new document for first occurrence of the given text
@type search_text: C{str} (Default: C{Empty String})
'''
QMainWindow.__init__(self, parent)
self.setObjectName(' - '.join(['Editor', str(filenames)]))
self.setAttribute(Qt.WA_DeleteOnClose, True)
self.setWindowFlags(Qt.Window)
self.mIcon = QIcon(":/icons/crystal_clear_edit_launch.png")
self._error_icon = QIcon(":/icons/crystal_clear_warning.png")
self._empty_icon = QIcon()
self.setWindowIcon(self.mIcon)
window_title = "ROSLaunch Editor"
if filenames:
window_title = self.__getTabName(filenames[0])
self.setWindowTitle(window_title)
self.init_filenames = list(filenames)
self._search_thread = None
# list with all open files
self.files = []
# create tabs for files
self.main_widget = QWidget(self)
self.verticalLayout = QVBoxLayout(self.main_widget)
self.verticalLayout.setContentsMargins(0, 0, 0, 0)
self.verticalLayout.setSpacing(1)
self.verticalLayout.setObjectName("verticalLayout")
self.tabWidget = EditorTabWidget(self)
self.tabWidget.setTabPosition(QTabWidget.North)
self.tabWidget.setDocumentMode(True)
self.tabWidget.setTabsClosable(True)
self.tabWidget.setMovable(False)
self.tabWidget.setObjectName("tabWidget")
self.tabWidget.tabCloseRequested.connect(self.on_close_tab)
self.verticalLayout.addWidget(self.tabWidget)
self.buttons = self._create_buttons()
self.verticalLayout.addWidget(self.buttons)
self.setCentralWidget(self.main_widget)
self.find_dialog = TextSearchFrame(self.tabWidget, self)
self.find_dialog.search_result_signal.connect(self.on_search_result)
self.find_dialog.replace_signal.connect(self.on_replace)
self.addDockWidget(Qt.RightDockWidgetArea, self.find_dialog)
# open the files
for f in filenames:
if f:
self.on_load_request(os.path.normpath(f), search_text)
self.readSettings()
self.find_dialog.setVisible(False)
# def __del__(self):
# print "******** destroy", self.objectName()
def _create_buttons(self):
# create the buttons line
self.buttons = QWidget(self)
self.horizontalLayout = QHBoxLayout(self.buttons)
self.horizontalLayout.setContentsMargins(4, 0, 4, 0)
self.horizontalLayout.setObjectName("horizontalLayout")
# add the search button
self.searchButton = QPushButton(self)
self.searchButton.setObjectName("searchButton")
# self.searchButton.clicked.connect(self.on_shortcut_find)
self.searchButton.toggled.connect(self.on_toggled_find)
self.searchButton.setText(self._translate("&Find"))
self.searchButton.setToolTip('Open a search dialog (Ctrl+F)')
self.searchButton.setFlat(True)
self.searchButton.setCheckable(True)
self.horizontalLayout.addWidget(self.searchButton)
# add the replace button
self.replaceButton = QPushButton(self)
self.replaceButton.setObjectName("replaceButton")
# self.replaceButton.clicked.connect(self.on_shortcut_replace)
self.replaceButton.toggled.connect(self.on_toggled_replace)
self.replaceButton.setText(self._translate("&Replace"))
self.replaceButton.setToolTip('Open a search&replace dialog (Ctrl+R)')
self.replaceButton.setFlat(True)
self.replaceButton.setCheckable(True)
self.horizontalLayout.addWidget(self.replaceButton)
# add the goto button
self.gotoButton = QPushButton(self)
self.gotoButton.setObjectName("gotoButton")
self.gotoButton.clicked.connect(self.on_shortcut_goto)
self.gotoButton.setText(self._translate("&Goto line"))
self.gotoButton.setShortcut("Ctrl+G")
self.gotoButton.setToolTip('Open a goto dialog (Ctrl+G)')
self.gotoButton.setFlat(True)
self.horizontalLayout.addWidget(self.gotoButton)
# add a tag button
self.tagButton = self._create_tag_button(self)
self.horizontalLayout.addWidget(self.tagButton)
# add spacer
spacerItem = QSpacerItem(515, 20, QSizePolicy.Expanding, QSizePolicy.Minimum)
self.horizontalLayout.addItem(spacerItem)
# add line number label
self.pos_label = QLabel()
self.horizontalLayout.addWidget(self.pos_label)
# add spacer
spacerItem = QSpacerItem(515, 20, QSizePolicy.Expanding, QSizePolicy.Minimum)
self.horizontalLayout.addItem(spacerItem)
# add save button
self.saveButton = QPushButton(self)
self.saveButton.setObjectName("saveButton")
self.saveButton.clicked.connect(self.on_saveButton_clicked)
self.saveButton.setText(self._translate("&Save"))
self.saveButton.setShortcut("Ctrl+S")
self.saveButton.setToolTip('Save the changes to the file (Ctrl+S)')
self.saveButton.setFlat(True)
self.horizontalLayout.addWidget(self.saveButton)
return self.buttons
def keyPressEvent(self, event):
'''
Enable the shortcats for search and replace
'''
if event.key() == Qt.Key_Escape:
self.reject()
elif event.modifiers() == Qt.ControlModifier and event.key() == Qt.Key_F:
if self.tabWidget.currentWidget().hasFocus():
if not self.searchButton.isChecked():
self.searchButton.setChecked(True)
else:
self.on_toggled_find(True)
else:
self.searchButton.setChecked(not self.searchButton.isChecked())
elif event.modifiers() == Qt.ControlModifier and event.key() == Qt.Key_R:
if self.tabWidget.currentWidget().hasFocus():
if not self.replaceButton.isChecked():
self.replaceButton.setChecked(True)
else:
self.on_toggled_replace(True)
else:
self.replaceButton.setChecked(not self.replaceButton.isChecked())
else:
event.accept()
QMainWindow.keyPressEvent(self, event)
def _translate(self, text):
if hasattr(QApplication, "UnicodeUTF8"):
return QApplication.translate("Editor", text, None, QApplication.UnicodeUTF8)
else:
return QApplication.translate("Editor", text, None)
def readSettings(self):
if nm.settings().store_geometry:
settings = nm.settings().qsettings(nm.settings().CFG_GUI_FILE)
settings.beginGroup("editor")
maximized = settings.value("maximized", 'false') == 'true'
if maximized:
self.showMaximized()
else:
self.resize(settings.value("size", QSize(800, 640)))
self.move(settings.value("pos", QPoint(0, 0)))
try:
self.restoreState(settings.value("window_state"))
except:
import traceback
print traceback.format_exc()
settings.endGroup()
def storeSetting(self):
if nm.settings().store_geometry:
settings = nm.settings().qsettings(nm.settings().CFG_GUI_FILE)
settings.beginGroup("editor")
settings.setValue("size", self.size())
settings.setValue("pos", self.pos())
settings.setValue("maximized", self.isMaximized())
settings.setValue("window_state", self.saveState())
settings.endGroup()
def on_load_request(self, filename, search_text=''):
'''
Loads a file in a new tab or focus the tab, if the file is already open.
@param filename: the path to file
@type filename: C{str}
@param search_text: if not empty, searches in new document for first occurrence of the given text
@type search_text: C{str} (Default: C{Empty String})
'''
if not filename:
return
self.tabWidget.setUpdatesEnabled(False)
try:
if filename not in self.files:
tab_name = self.__getTabName(filename)
editor = TextEdit(filename, self.tabWidget)
linenumber_editor = LineNumberWidget(editor)
tab_index = self.tabWidget.addTab(linenumber_editor, tab_name)
self.files.append(filename)
editor.setCurrentPath(os.path.basename(filename))
editor.load_request_signal.connect(self.on_load_request)
editor.document().modificationChanged.connect(self.on_editor_modificationChanged)
editor.cursorPositionChanged.connect(self.on_editor_positionChanged)
editor.setFocus(Qt.OtherFocusReason)
editor.textChanged.connect(self.on_text_changed)
self.tabWidget.setCurrentIndex(tab_index)
# self.find_dialog.set_search_path(filename)
else:
for i in range(self.tabWidget.count()):
if self.tabWidget.widget(i).filename == filename:
self.tabWidget.setCurrentIndex(i)
break
except:
import traceback
rospy.logwarn("Error while open %s: %s", filename, traceback.format_exc(1))
self.tabWidget.setUpdatesEnabled(True)
if search_text:
try:
self._search_thread.stop()
self._search_thread = None
except:
pass
self._search_thread = TextSearchThread(search_text, filename, path_text=self.tabWidget.widget(0).document().toPlainText(), recursive=True)
self._search_thread.search_result_signal.connect(self.on_search_result_on_open)
self._search_thread.start()
def on_text_changed(self):
if self.tabWidget.currentWidget().hasFocus():
self.find_dialog.file_changed(self.tabWidget.currentWidget().filename)
def on_close_tab(self, tab_index):
'''
Signal handling to close single tabs.
@param tab_index: tab index to close
@type tab_index: C{int}
'''
try:
doremove = True
w = self.tabWidget.widget(tab_index)
if w.document().isModified():
name = self.__getTabName(w.filename)
result = QMessageBox.question(self, "Unsaved Changes", '\n\n'.join(["Save the file before closing?", name]), QMessageBox.Yes | QMessageBox.No | QMessageBox.Cancel)
if result == QMessageBox.Yes:
self.tabWidget.currentWidget().save()
elif result == QMessageBox.No:
pass
else:
doremove = False
if doremove:
# remove the indexed files
if w.filename in self.files:
self.files.remove(w.filename)
# close tab
self.tabWidget.removeTab(tab_index)
# close editor, if no tabs are open
if not self.tabWidget.count():
self.close()
except:
import traceback
rospy.logwarn("Error while close tab %s: %s", str(tab_index), traceback.format_exc(1))
def reject(self):
if self.find_dialog.isVisible():
self.searchButton.setChecked(not self.searchButton.isChecked())
else:
self.close()
def closeEvent(self, event):
'''
Test the open files for changes and save this if needed.
'''
changed = []
# get the names of all changed files
for i in range(self.tabWidget.count()):
w = self.tabWidget.widget(i)
if w.document().isModified():
changed.append(self.__getTabName(w.filename))
if changed:
# ask the user for save changes
if self.isHidden():
buttons = QMessageBox.Yes | QMessageBox.No
else:
buttons = QMessageBox.Yes | QMessageBox.No | QMessageBox.Cancel
result = QMessageBox.question(self, "Unsaved Changes", '\n\n'.join(["Save the file before closing?", '\n'.join(changed)]), buttons)
if result == QMessageBox.Yes:
for i in range(self.tabWidget.count()):
w = self.tabWidget.widget(i).save()
event.accept()
elif result == QMessageBox.No:
event.accept()
else:
event.ignore()
else:
event.accept()
if event.isAccepted():
self.storeSetting()
self.finished_signal.emit(self.init_filenames)
def on_editor_modificationChanged(self, value=None):
'''
If the content was changed, a '*' will be shown in the tab name.
'''
tab_name = self.__getTabName(self.tabWidget.currentWidget().filename)
if (self.tabWidget.currentWidget().document().isModified()) or not QFileInfo(self.tabWidget.currentWidget().filename).exists():
tab_name = ''.join(['*', tab_name])
self.tabWidget.setTabText(self.tabWidget.currentIndex(), tab_name)
def on_editor_positionChanged(self):
'''
Shows the number of the line and column in a label.
'''
cursor = self.tabWidget.currentWidget().textCursor()
self.pos_label.setText(':%s:%s' % (cursor.blockNumber() + 1, cursor.columnNumber()))
def __getTabName(self, lfile):
base = os.path.basename(lfile).replace('.launch', '')
(package, _) = package_name(os.path.dirname(lfile))
return '%s [%s]' % (base, package)
##############################################################################
# HANDLER for buttons
##############################################################################
def on_saveButton_clicked(self):
'''
Saves the current document. This method is called if the C{save button}
was clicked.
'''
saved, errors, msg = self.tabWidget.currentWidget().save(True)
if errors:
QMessageBox.critical(self, "Error", msg)
self.tabWidget.setTabIcon(self.tabWidget.currentIndex(), self._error_icon)
self.tabWidget.setTabToolTip(self.tabWidget.currentIndex(), msg)
elif saved:
self.tabWidget.setTabIcon(self.tabWidget.currentIndex(), self._empty_icon)
self.tabWidget.setTabToolTip(self.tabWidget.currentIndex(), '')
def on_shortcut_find(self):
pass
def on_toggled_find(self, value):
'''
Shows the search frame
'''
if value:
self.find_dialog.enable()
else:
self.replaceButton.setChecked(False)
self.find_dialog.setVisible(False)
self.tabWidget.currentWidget().setFocus()
def on_toggled_replace(self, value):
'''
Shows the replace lineedit in the search frame
'''
if value:
self.searchButton.setChecked(True)
self.find_dialog.set_replace_visible(value)
def on_shortcut_goto(self):
'''
Opens a C{goto} dialog.
'''
value = 1
ok = False
try:
value, ok = QInputDialog.getInt(self, "Goto", self.tr("Line number:"),
QLineEdit.Normal, minValue=1, step=1)
except:
value, ok = QInputDialog.getInt(self, "Goto", self.tr("Line number:"),
QLineEdit.Normal, min=1, step=1)
if ok:
if value > self.tabWidget.currentWidget().document().blockCount():
value = self.tabWidget.currentWidget().document().blockCount()
curpos = self.tabWidget.currentWidget().textCursor().blockNumber() + 1
while curpos != value:
mov = QTextCursor.NextBlock if curpos < value else QTextCursor.PreviousBlock
self.tabWidget.currentWidget().moveCursor(mov)
curpos = self.tabWidget.currentWidget().textCursor().blockNumber() + 1
self.tabWidget.currentWidget().setFocus(Qt.ActiveWindowFocusReason)
##############################################################################
# SLOTS for search dialog
##############################################################################
def on_search_result(self, search_text, found, path, index):
'''
A slot to handle a found text. It goes to the position in the text and select
the searched text. On new file it will be open.
:param search_text: the searched text
:type search_text: str
:param found: the text was found or not
:type found: bool
:param path: the path of the file the text was found
:type path: str
:param index: the position in the text
:type index: int
'''
if found:
if self.tabWidget.currentWidget().filename != path:
focus_widget = QApplication.focusWidget()
self.on_load_request(path)
focus_widget.setFocus()
cursor = self.tabWidget.currentWidget().textCursor()
cursor.setPosition(index, QTextCursor.MoveAnchor)
cursor.movePosition(QTextCursor.NextCharacter, QTextCursor.KeepAnchor, len(search_text))
self.tabWidget.currentWidget().setTextCursor(cursor)
def on_search_result_on_open(self, search_text, found, path, index):
'''
Like on_search_result, but skips the text in comments.
'''
if found:
if self.tabWidget.currentWidget().filename != path:
focus_widget = QApplication.focusWidget()
self.on_load_request(path)
focus_widget.setFocus()
comment_start = self.tabWidget.currentWidget().document().find('<!--', index, QTextDocument.FindBackward)
if not comment_start.isNull():
comment_end = self.tabWidget.currentWidget().document().find('-->', comment_start)
if not comment_end.isNull() and comment_end.position() > index + len(search_text):
# commented -> retrun
return
self.on_search_result(search_text, found, path, index)
def on_replace(self, search_text, path, index, replaced_text):
'''
A slot to handle a text replacement of the TextSearchFrame.
:param search_text: the searched text
:type search_text: str
:param path: the path of the file the text was found
:type path: str
:param index: the position in the text
:type index: int
:param replaced_text: the new text
:type replaced_text: str
'''
cursor = self.tabWidget.currentWidget().textCursor()
if cursor.selectedText() == search_text:
cursor.insertText(replaced_text)
##############################################################################
# LAUNCH TAG insertion
##############################################################################
def _create_tag_button(self, parent=None):
btn = QPushButton(parent)
btn.setObjectName("tagButton")
btn.setText(self._translate("Add &tag"))
btn.setShortcut("Ctrl+T")
btn.setToolTip('Adds a ROS launch tag to launch file (Ctrl+T)')
btn.setMenu(self._create_tag_menu(btn))
btn.setFlat(True)
return btn
def _create_tag_menu(self, parent=None):
# creates a tag menu
tag_menu = QMenu("ROS Tags", parent)
# group tag
add_group_tag_action = QAction("<group>", self, statusTip="", triggered=self._on_add_group_tag)
add_group_tag_action.setShortcuts(QKeySequence("Ctrl+Shift+g"))
tag_menu.addAction(add_group_tag_action)
# node tag
add_node_tag_action = QAction("<node>", self, statusTip="", triggered=self._on_add_node_tag)
add_node_tag_action.setShortcuts(QKeySequence("Ctrl+Shift+n"))
tag_menu.addAction(add_node_tag_action)
# node tag with all attributes
add_node_tag_all_action = QAction("<node all>", self, statusTip="", triggered=self._on_add_node_tag_all)
tag_menu.addAction(add_node_tag_all_action)
# include tag with all attributes
add_include_tag_all_action = QAction("<include>", self, statusTip="", triggered=self._on_add_include_tag_all)
add_include_tag_all_action.setShortcuts(QKeySequence("Ctrl+Shift+i"))
tag_menu.addAction(add_include_tag_all_action)
# remap
add_remap_tag_action = QAction("<remap>", self, statusTip="", triggered=self._on_add_remap_tag)
add_remap_tag_action.setShortcuts(QKeySequence("Ctrl+Shift+r"))
tag_menu.addAction(add_remap_tag_action)
# env tag
add_env_tag_action = QAction("<env>", self, statusTip="", triggered=self._on_add_env_tag)
tag_menu.addAction(add_env_tag_action)
# param tag
add_param_tag_action = QAction("<param>", self, statusTip="", triggered=self._on_add_param_tag)
add_param_tag_action.setShortcuts(QKeySequence("Ctrl+Shift+p"))
tag_menu.addAction(add_param_tag_action)
# param capability group tag
add_param_cap_group_tag_action = QAction("<param capability group>", self, statusTip="", triggered=self._on_add_param_cap_group_tag)
add_param_cap_group_tag_action.setShortcuts(QKeySequence("Ctrl+Alt+p"))
tag_menu.addAction(add_param_cap_group_tag_action)
# param tag with all attributes
add_param_tag_all_action = QAction("<param all>", self, statusTip="", triggered=self._on_add_param_tag_all)
tag_menu.addAction(add_param_tag_all_action)
# rosparam tag with all attributes
add_rosparam_tag_all_action = QAction("<rosparam>", self, statusTip="", triggered=self._on_add_rosparam_tag_all)
tag_menu.addAction(add_rosparam_tag_all_action)
# arg tag with default definition
add_arg_tag_default_action = QAction("<arg default>", self, statusTip="", triggered=self._on_add_arg_tag_default)
add_arg_tag_default_action.setShortcuts(QKeySequence("Ctrl+Shift+a"))
tag_menu.addAction(add_arg_tag_default_action)
# arg tag with value definition
add_arg_tag_value_action = QAction("<arg value>", self, statusTip="", triggered=self._on_add_arg_tag_value)
add_arg_tag_value_action.setShortcuts(QKeySequence("Ctrl+Alt+a"))
tag_menu.addAction(add_arg_tag_value_action)
# test tag
add_test_tag_action = QAction("<test>", self, statusTip="", triggered=self._on_add_test_tag)
add_test_tag_action.setShortcuts(QKeySequence("Ctrl+Alt+t"))
tag_menu.addAction(add_test_tag_action)
# test tag with all attributes
add_test_tag_all_action = QAction("<test all>", self, statusTip="", triggered=self._on_add_test_tag_all)
tag_menu.addAction(add_test_tag_all_action)
return tag_menu
def _insert_text(self, text):
cursor = self.tabWidget.currentWidget().textCursor()
if not cursor.isNull():
col = cursor.columnNumber()
spaces = ''.join([' ' for _ in range(col)])
cursor.insertText(text.replace('\n', '\n%s' % spaces))
self.tabWidget.currentWidget().setFocus(Qt.OtherFocusReason)
def _on_add_group_tag(self):
self._insert_text('<group ns="namespace" clear_params="true|false">\n'
'</group>')
def _on_add_node_tag(self):
dia = PackageDialog()
if dia.exec_():
self._insert_text('<node name="%s" pkg="%s" type="%s">\n'
'</node>' % (dia.binary, dia.package, dia.binary))
def _on_add_node_tag_all(self):
dia = PackageDialog()
if dia.exec_():
self._insert_text('<node name="%s" pkg="%s" type="%s"\n'
' args="arg1" machine="machine-name"\n'
' respawn="true" required="true"\n'
' ns="foo" clear_params="true|false"\n'
' output="log|screen" cwd="ROS_HOME|node"\n'
' launch-prefix="prefix arguments">\n'
'</node>' % (dia.binary, dia.package, dia.binary))
def _on_add_include_tag_all(self):
self._insert_text('<include file="$(find pkg-name)/path/filename.xml"\n'
' ns="foo" clear_params="true|false">\n'
'</include>')
def _on_add_remap_tag(self):
self._insert_text('<remap from="original" to="new"/>')
def _on_add_env_tag(self):
self._insert_text('<env name="variable" value="value"/>')
def _on_add_param_tag(self):
self._insert_text('<param name="ns_name" value="value" />')
def _on_add_param_cap_group_tag(self):
self._insert_text('<param name="capability_group" value="demo" />')
def _on_add_param_tag_all(self):
self._insert_text('<param name="ns_name" value="value"\n'
' type="str|int|double|bool"\n'
' textfile="$(find pkg-name)/path/file.txt"\n'
' binfile="$(find pkg-name)/path/file"\n'
' command="$(find pkg-name)/exe \'$(find pkg-name)/arg.txt\'">\n'
'</param>')
def _on_add_rosparam_tag_all(self):
self._insert_text('<rosparam param="param-name"\n'
' file="$(find pkg-name)/path/foo.yaml"\n'
' command="load|dump|delete"\n'
' ns="namespace">\n'
'</rosparam>')
def _on_add_arg_tag_default(self):
self._insert_text('<arg name="foo" default="1" />')
def _on_add_arg_tag_value(self):
self._insert_text('<arg name="foo" value="bar" />')
def _on_add_test_tag(self):
dia = PackageDialog()
if dia.exec_():
self._insert_text('<test name="%s" pkg="%s" type="%s" test-name="test_%s">\n'
'</test>' % (dia.binary, dia.package, dia.binary, dia.binary))
def _on_add_test_tag_all(self):
dia = PackageDialog()
if dia.exec_():
self._insert_text('<test name="%s" pkg="%s" type="%s" test-name="test_%s">\n'
' args="arg1" time-limit="60.0"\n'
' ns="foo" clear_params="true|false"\n'
' cwd="ROS_HOME|node" retry="0"\n'
' launch-prefix="prefix arguments">\n'
'</test>' % (dia.binary, dia.package, dia.binary, dia.binary))
class TextSearchFrame(QDockWidget):
'''
A frame to find text in the Editor.
'''
search_result_signal = Signal(str, bool, str, int)
''' @ivar: A signal emitted after search_threaded was started.
(search text, found or not, file, position in text)
for each result a signal will be emitted.
'''
replace_signal = Signal(str, str, int, str)
''' @ivar: A signal emitted to replace string at given position.
(search text, file, position in text, replaced by text)
'''
def __init__(self, tabwidget, parent=None):
QDockWidget.__init__(self, "Find", parent)
self.setObjectName('SearchFrame')
self.setFeatures(QDockWidget.DockWidgetMovable | QDockWidget.DockWidgetFloatable)
self._dockwidget = QFrame(self)
self.vbox_layout = QVBoxLayout(self._dockwidget)
self.layout().setContentsMargins(0, 0, 0, 0)
self.layout().setSpacing(1)
# frame with two rows for find and replace
find_replace_frame = QFrame(self)
find_replace_vbox_layout = QVBoxLayout(find_replace_frame)
find_replace_vbox_layout.setContentsMargins(0, 0, 0, 0)
find_replace_vbox_layout.setSpacing(1)
# find_replace_vbox_layout.addSpacerItem(QSpacerItem(1, 1, QSizePolicy.Expanding, QSizePolicy.Expanding))
# create frame with find row
find_frame = self._create_find_frame()
find_replace_vbox_layout.addWidget(find_frame)
rplc_frame = self._create_replace_frame()
find_replace_vbox_layout.addWidget(rplc_frame)
# frame for find&replace and search results
self.vbox_layout.addWidget(find_replace_frame)
self.vbox_layout.addWidget(self._create_found_frame())
# self.vbox_layout.addStretch(2024)
self.setWidget(self._dockwidget)
# intern search parameters
self._tabwidget = tabwidget
self.current_search_text = ''
self.search_results = []
self.search_results_fileset = set()
self._search_result_index = -1
self._search_recursive = False
self._search_thread = None
def _create_find_frame(self):
find_frame = QFrame(self)
find_hbox_layout = QHBoxLayout(find_frame)
find_hbox_layout.setContentsMargins(0, 0, 0, 0)
find_hbox_layout.setSpacing(1)
self.search_field = EnchancedLineEdit(find_frame)
self.search_field.setPlaceholderText('search text')
self.search_field.textChanged.connect(self.on_search_text_changed)
self.search_field.returnPressed.connect(self.on_search)
find_hbox_layout.addWidget(self.search_field)
self.search_result_label = QLabel(find_frame)
self.search_result_label.setText(' ')
find_hbox_layout.addWidget(self.search_result_label)
self.find_button_back = QPushButton("<")
self.find_button_back.setFixedWidth(44)
self.find_button_back.clicked.connect(self.on_search_back)
find_hbox_layout.addWidget(self.find_button_back)
self.find_button = QPushButton(">")
self.find_button.setDefault(True)
# self.find_button.setFlat(True)
self.find_button.setFixedWidth(44)
self.find_button.clicked.connect(self.on_search)
find_hbox_layout.addWidget(self.find_button)
return find_frame
def _create_replace_frame(self):
# create frame with replace row
self.rplc_frame = rplc_frame = QFrame(self)
rplc_hbox_layout = QHBoxLayout(rplc_frame)
rplc_hbox_layout.setContentsMargins(0, 0, 0, 0)
rplc_hbox_layout.setSpacing(1)
self.replace_field = EnchancedLineEdit(rplc_frame)
self.replace_field.setPlaceholderText('replace text')
self.replace_field.returnPressed.connect(self.on_replace)
rplc_hbox_layout.addWidget(self.replace_field)
self.replace_result_label = QLabel(rplc_frame)
self.replace_result_label.setText(' ')
rplc_hbox_layout.addWidget(self.replace_result_label)
self.replace_button = replace_button = QPushButton("> &Replace >")
replace_button.setFixedWidth(90)
replace_button.clicked.connect(self.on_replace_click)
rplc_hbox_layout.addWidget(replace_button)
rplc_frame.setVisible(False)
return rplc_frame
def _create_found_frame(self):
ff_frame = QFrame(self)
self.found_files_vbox_layout = QVBoxLayout(ff_frame)
self.found_files_vbox_layout.setContentsMargins(0, 0, 0, 0)
self.recursive_search_box = QCheckBox("recursive search")
self.found_files_vbox_layout.addWidget(self.recursive_search_box)
self.found_files_list = QTreeWidget(ff_frame)
self.found_files_list.setColumnCount(1)
self.found_files_list.setFrameStyle(QFrame.StyledPanel)
self.found_files_list.setHeaderHidden(True)
self.found_files_list.itemActivated.connect(self.on_itemActivated)
self.found_files_list.setStyleSheet(
"QTreeWidget {"
"background-color:transparent;"
"}"
"QTreeWidget::item {"
"background-color:transparent;"
"}"
"QTreeWidget::item:selected {"
"background-color: darkgray;"
"}")
self.found_files_vbox_layout.addWidget(self.found_files_list)
self.recursive_search_box.setChecked(False)
return ff_frame
def keyPressEvent(self, event):
'''
Enable the shortcats for search and replace
'''
self.parent().keyPressEvent(event)
def on_search(self):
'''
Initiate the new search or request a next search result.
'''
if self.current_search_text != self.search_field.text() or self._search_recursive != self.recursive_search_box.isChecked():
# clear current search results
self._reset()
self.current_search_text = self.search_field.text()
if self.current_search_text:
path_text = {}
self._wait_for_result = True
for i in range(self._tabwidget.count()):
path_text[self._tabwidget.widget(i).filename] = self._tabwidget.widget(i).document().toPlainText()
self._search_recursive = self.recursive_search_box.isChecked()
self._search_thread = TextSearchThread(self.current_search_text, self._tabwidget.currentWidget().filename, path_text=path_text, recursive=self._search_recursive)
self._search_thread.search_result_signal.connect(self.on_search_result)
self._search_thread.warning_signal.connect(self.on_warning_result)
self._search_thread.start()
elif self.search_results:
self._check_position()
if self.search_results:
if self._search_result_index + 1 >= len(self.search_results):
self._search_result_index = -1
self._search_result_index += 1
(id, search_text, found, path, index, linenr, line) = self.search_results[self._search_result_index]
self.search_result_signal.emit(search_text, found, path, index)
self.replace_button.setEnabled(True)
self._update_label()
def on_search_back(self):
'''
Slot to handle the search back function.
'''
self._check_position(False)
if self.search_results:
self._search_result_index -= 1
if self._search_result_index < 0:
self._search_result_index = len(self.search_results) - 1
self._update_label()
(id, search_text, found, path, index, linenr, line) = self.search_results[self._search_result_index]
self.search_result_signal.emit(search_text, found, path, index)
self.replace_button.setEnabled(True)
def _check_position(self, forward=True):
try:
# if the position of the textCursor was changed by the user, move the search index
cur_pos = self._tabwidget.currentWidget().textCursor().position()
id, st, _f, pa, idx, lnr, ltxt = self.search_results[self._search_result_index]
sear_pos = idx + len(st)
if cur_pos != sear_pos:
first_idx = self._get_current_index_for_current_file()
if first_idx != -1:
id, st, _f, pa, idx, lnr, ltxt = self.search_results[first_idx]
sear_pos = idx + len(st)
while cur_pos > sear_pos and self._tabwidget.currentWidget().filename == pa:
first_idx += 1
id, st, _f, pa, idx, lnr, ltxt = self.search_results[first_idx]
sear_pos = idx + len(st)
self._search_result_index = first_idx
if forward:
self._search_result_index -= 1
else:
self._reset(True)
except:
pass
def _get_current_index_for_current_file(self):
for index in range(len(self.search_results)):
id, _st, _f, pa, _idx = self.search_results[index]
if self._tabwidget.currentWidget().filename == pa:
return index
return -1
def on_search_result(self, search_text, found, path, index, linenr, line):
'''
Slot to handle the signals for search result. This signals are forwarded used
search_result_signal.
'''
if found and search_text == self.current_search_text:
id = "%d:%s" % (index, path)
self.search_results_fileset.add(path)
item = (search_text, found, path, index)
if item not in self.search_results:
self.search_results.append((id, search_text, found, path, index, linenr, line))
if self._wait_for_result:
self._search_result_index += 1
if index >= self._tabwidget.currentWidget().textCursor().position() or self._tabwidget.currentWidget().filename != path:
self._wait_for_result = False
self.search_result_signal.emit(search_text, found, path, index)
self.replace_button.setEnabled(True)
pkg, rpath = package_name(os.path.dirname(path))
itemstr = '%s [%s]' % (os.path.basename(path), pkg)
if not self.found_files_list.findItems(itemstr, Qt.MatchExactly):
list_item = QTreeWidgetItem(self.found_files_list)
list_item.setText(0, itemstr)
list_item.setToolTip(0, path)
self.found_files_list.insertTopLevelItem(0, list_item)
self.found_files_list.expandAll()
for i in range(self.found_files_list.topLevelItemCount()):
top_item = self.found_files_list.topLevelItem(i)
if top_item.text(0) == itemstr:
sub_item_str = "%d: %s" % (linenr, line)
list_item2 = QTreeWidgetItem()
list_item2.setText(0, sub_item_str)
list_item2.setWhatsThis(0, id)
top_item.addChild(list_item2)
#self.found_files_list.setVisible(len(self.search_results_fileset) > 0)
self._update_label()
def on_warning_result(self, text):
rospy.logwarn(text)
def on_replace_click(self):
self.on_replace()
self.on_search()
def on_replace(self):
'''
Emits the replace signal, but only if currently selected text is equal to the searched one.
'''
if self.search_results:
try:
id, search_text, _found, path, index, linenr, line_text = self.search_results[self._search_result_index]
cursor = self._tabwidget.currentWidget().textCursor()
if cursor.selectedText() == search_text:
rptxt = self.replace_field.text()
for rindex in range(self._search_result_index + 1, len(self.search_results)):
iid, st, _f, pa, idx, lnr, ltxt = self.search_results[rindex]
if path == pa:
self.search_results.pop(rindex)
self.search_results.insert(rindex, (iid, st, _f, pa, idx + len(rptxt) - len(st), lnr, ltxt))
else:
break
self._remove_search_result(self._search_result_index)
self._search_result_index -= 1
self.replace_signal.emit(search_text, path, index, rptxt)
else:
self.replace_button.setEnabled(False)
except:
import traceback
print traceback.format_exc()
pass
def on_itemActivated(self, item):
'''
Go to the results for the selected file entry in the list.
'''
splits = item.whatsThis(0).split(':')
if len(splits) == 2:
item_index = int(splits[0])
item_path = splits[1]
new_search_index = -1
tmp_index = -1
search_index = -1
tmp_search_text = ''
for id, search_text, found, path, index, linenr, line_text in self.search_results:
new_search_index += 1
if item_path == path and item_index == index:
self._search_result_index = new_search_index
self.search_result_signal.emit(search_text, found, path, index)
self._update_label()
def on_search_text_changed(self, _text):
'''
Clear search result if the text was changed.
'''
self._reset()
def _update_label(self, clear_label=False):
'''
Updates the status label for search results. The info is created from search result lists.
'''
msg = ' '
if self.search_results:
count_files = len(self.search_results_fileset)
msg = '%d/%d' % (self._search_result_index + 1, len(self.search_results))
if count_files > 1:
msg = '%s(%d)' % (msg, count_files)
if self._search_thread is not None and self._search_thread.is_alive():
msg = 'searching..%s' % msg
elif not msg.strip() and self.current_search_text:
msg = '0 found'
self.current_search_text = ''
if clear_label:
msg = ' '
self.search_result_label.setText(msg)
self.find_button_back.setEnabled(len(self.search_results))
self._select_current_item_in_box(self._search_result_index)
def file_changed(self, path):
'''
Clears search results if for changed file are some search results are available
:param path: changed file path
:type path: str
'''
if path in self.search_results_fileset:
self._reset()
def set_replace_visible(self, value):
self.rplc_frame.setVisible(value)
self.raise_()
self.activateWindow()
if value:
self.replace_field.setFocus()
self.replace_field.selectAll()
self.setWindowTitle("Find / Replace")
else:
self.setWindowTitle("Find")
self.search_field.setFocus()
def is_replace_visible(self):
return self.rplc_frame.isVisible()
def _reset(self, force_new_search=False):
# clear current search results
if self._search_thread is not None:
self._search_thread.search_result_signal.disconnect()
self._search_thread.stop()
self._search_thread = None
self.current_search_text = ''
self.search_results = []
self.search_results_fileset = set()
self.found_files_list.clear()
# self.found_files_list.setVisible(False)
self._update_label(True)
self._search_result_index = -1
self.find_button_back.setEnabled(False)
if force_new_search:
self.on_search()
def enable(self):
self.setVisible(True)
# self.show()
self.raise_()
self.activateWindow()
self.search_field.setFocus()
self.search_field.selectAll()
def _select_current_item_in_box(self, index):
try:
(id, search_text, found, path, index, linenr, line) = self.search_results[index]
for topidx in range(self.found_files_list.topLevelItemCount()):
topitem = self.found_files_list.topLevelItem(topidx)
for childdx in range(topitem.childCount()):
child = topitem.child(childdx)
if child.whatsThis(0) == id:
child.setSelected(True)
elif child.isSelected():
child.setSelected(False)
except:
pass
def _remove_search_result(self, index):
try:
(id, search_text, found, path, index, linenr, line) = self.search_results.pop(index)
pkg, rpath = package_name(os.path.dirname(path))
itemstr = '%s [%s]' % (os.path.basename(path), pkg)
found_items = self.found_files_list.findItems(itemstr, Qt.MatchExactly)
for item in found_items:
for chi in range(item.childCount()):
child = item.child(chi)
if child.whatsThis(0) == id:
item.removeChild(child)
break
# delete top level item if it is now empty
for topidx in range(self.found_files_list.topLevelItemCount()):
if self.found_files_list.topLevelItem(topidx).childCount() == 0:
self.found_files_list.takeTopLevelItem(topidx)
break
# create new set with files contain the search text
new_path_set = set(path for _id, _st, _fd, path, _idx, lnr, lntxt in self.search_results)
self.search_results_fileset = new_path_set
# self.found_files_list.setVisible(len(self.search_results_fileset) > 0)
except:
import traceback
print traceback.format_exc()
class EchoDialog(QDialog):
MESSAGE_LINE_LIMIT = 128
MESSAGE_HZ_LIMIT = 10
MAX_DISPLAY_MSGS = 25
STATISTIC_QUEUE_LEN = 5000
'''
This dialog shows the output of a topic.
'''
finished_signal = Signal(str)
'''
finished_signal has as parameter the name of the topic and is emitted, if this
dialog was closed.
'''
msg_signal = Signal(object, bool)
'''
msg_signal is a signal, which is emitted, if a new message was received.
'''
text_hz_signal = Signal(str)
text_signal = Signal(str)
'''
text_signal is a signal, which is emitted, if a new text to display was received.
'''
text_error_signal = Signal(str)
'''
text_error_signal is a signal, which is emitted, if a new error text to display was received.
'''
request_pw = Signal(object)
def __init__(self,
topic,
msg_type,
show_only_rate=False,
masteruri=None,
use_ssh=False,
parent=None):
'''
Creates an input dialog.
@param topic: the name of the topic
@type topic: C{str}
@param msg_type: the type of the topic
@type msg_type: C{str}
@raise Exception: if no topic class was found for the given type
'''
QDialog.__init__(self, parent=parent)
self._masteruri = masteruri
masteruri_str = '' if masteruri is None else '[%s]' % masteruri
self.setObjectName(' - '.join(['EchoDialog', topic, masteruri_str]))
self.setAttribute(Qt.WA_DeleteOnClose, True)
self.setWindowFlags(Qt.Window)
self.setWindowTitle('%s %s %s' %
('Echo --- ' if not show_only_rate else 'Hz --- ',
topic, masteruri_str))
self.resize(728, 512)
self.verticalLayout = QVBoxLayout(self)
self.verticalLayout.setObjectName("verticalLayout")
self.verticalLayout.setContentsMargins(1, 1, 1, 1)
self.mIcon = QIcon(":/icons/crystal_clear_prop_run_echo.png")
self.setWindowIcon(self.mIcon)
self.topic = topic
self.show_only_rate = show_only_rate
self.lock = threading.RLock()
self.last_printed_count = 0
self.msg_t0 = -1.
self.msg_tn = 0
self.times = []
self.message_count = 0
self._rate_message = ''
self._scrapped_msgs = 0
self._scrapped_msgs_sl = 0
self._last_received_ts = 0
self.receiving_hz = self.MESSAGE_HZ_LIMIT
self.line_limit = self.MESSAGE_LINE_LIMIT
self.field_filter_fn = None
options = QWidget(self)
if not show_only_rate:
hLayout = QHBoxLayout(options)
hLayout.setContentsMargins(1, 1, 1, 1)
self.no_str_checkbox = no_str_checkbox = QCheckBox('Hide strings')
no_str_checkbox.toggled.connect(self.on_no_str_checkbox_toggled)
hLayout.addWidget(no_str_checkbox)
self.no_arr_checkbox = no_arr_checkbox = QCheckBox('Hide arrays')
no_arr_checkbox.toggled.connect(self.on_no_arr_checkbox_toggled)
hLayout.addWidget(no_arr_checkbox)
self.combobox_reduce_ch = QComboBox(self)
self.combobox_reduce_ch.addItems(
[str(self.MESSAGE_LINE_LIMIT), '0', '80', '256', '1024'])
self.combobox_reduce_ch.activated[str].connect(
self.combobox_reduce_ch_activated)
self.combobox_reduce_ch.setEditable(True)
self.combobox_reduce_ch.setToolTip(
"Set maximum line width. 0 disables the limit.")
hLayout.addWidget(self.combobox_reduce_ch)
# reduce_ch_label = QLabel('ch', self)
# hLayout.addWidget(reduce_ch_label)
# add spacer
spacerItem = QSpacerItem(515, 20, QSizePolicy.Expanding,
QSizePolicy.Minimum)
hLayout.addItem(spacerItem)
# add combobox for displaying frequency of messages
self.combobox_displ_hz = QComboBox(self)
self.combobox_displ_hz.addItems([
str(self.MESSAGE_HZ_LIMIT), '0', '0.1', '1', '50', '100',
'1000'
])
self.combobox_displ_hz.activated[str].connect(
self.on_combobox_hz_activated)
self.combobox_displ_hz.setEditable(True)
hLayout.addWidget(self.combobox_displ_hz)
displ_hz_label = QLabel('Hz', self)
hLayout.addWidget(displ_hz_label)
# add combobox for count of displayed messages
self.combobox_msgs_count = QComboBox(self)
self.combobox_msgs_count.addItems(
[str(self.MAX_DISPLAY_MSGS), '0', '50', '100'])
self.combobox_msgs_count.activated[str].connect(
self.on_combobox_count_activated)
self.combobox_msgs_count.setEditable(True)
self.combobox_msgs_count.setToolTip(
"Set maximum displayed message count. 0 disables the limit.")
hLayout.addWidget(self.combobox_msgs_count)
displ_count_label = QLabel('#', self)
hLayout.addWidget(displ_count_label)
# add topic control button for unsubscribe and subscribe
self.topic_control_button = QToolButton(self)
self.topic_control_button.setText('stop')
self.topic_control_button.setIcon(
QIcon(':/icons/deleket_deviantart_stop.png'))
self.topic_control_button.clicked.connect(
self.on_topic_control_btn_clicked)
hLayout.addWidget(self.topic_control_button)
# add clear button
clearButton = QToolButton(self)
clearButton.setText('clear')
clearButton.clicked.connect(self.on_clear_btn_clicked)
hLayout.addWidget(clearButton)
self.verticalLayout.addWidget(options)
self.display = QTextBrowser(self)
self.display.setReadOnly(True)
self.verticalLayout.addWidget(self.display)
self.display.document().setMaximumBlockCount(500)
self.max_displayed_msgs = self.MAX_DISPLAY_MSGS
self._blocks_in_msg = None
self.display.setOpenLinks(False)
self.display.anchorClicked.connect(self._on_display_anchorClicked)
self.status_label = QLabel('0 messages', self)
self.verticalLayout.addWidget(self.status_label)
# subscribe to the topic
errmsg = ''
try:
self.__msg_class = message.get_message_class(msg_type)
if not self.__msg_class:
errmsg = "Cannot load message class for [%s]. Did you build messages?" % msg_type
# raise Exception("Cannot load message class for [%s]. Did you build messages?"%msg_type)
except Exception as e:
self.__msg_class = None
errmsg = "Cannot load message class for [%s]. Did you build messagest?\nError: %s" % (
msg_type, e)
# raise Exception("Cannot load message class for [%s]. Did you build messagest?\nError: %s"%(msg_type, e))
# variables for Subscriber
self.msg_signal.connect(self._append_message)
self.sub = None
# vairables for SSH connection
self.ssh_output_file = None
self.ssh_error_file = None
self.ssh_input_file = None
self.text_signal.connect(self._append_text)
self.text_hz_signal.connect(self._append_text_hz)
self._current_msg = ''
self._current_errmsg = ''
self.text_error_signal.connect(self._append_error_text)
# decide, which connection to open
if use_ssh:
self.__msg_class = None
self._on_display_anchorClicked(QUrl(self._masteruri))
elif self.__msg_class is None:
errtxt = '<pre style="color:red; font-family:Fixedsys,Courier,monospace; padding:10px;">\n%s</pre>' % (
errmsg)
self.display.setText('<a href="%s">open using SSH</a>' %
(masteruri))
self.display.append(errtxt)
else:
self.sub = rospy.Subscriber(self.topic, self.__msg_class,
self._msg_handle)
self.print_hz_timer = QTimer()
self.print_hz_timer.timeout.connect(self._on_calc_hz)
self.print_hz_timer.start(1000)
# print "======== create", self.objectName()
#
# def __del__(self):
# print "******* destroy", self.objectName()
# def hideEvent(self, event):
# self.close()
def closeEvent(self, event):
if self.sub is not None:
self.sub.unregister()
del self.sub
try:
self.ssh_output_file.close()
self.ssh_error_file.close()
# send Ctrl+C to remote process
self.ssh_input_file.write('%s\n' % chr(3))
self.ssh_input_file.close()
except:
pass
self.finished_signal.emit(self.topic)
if self.parent() is None:
QApplication.quit()
# else:
# self.setParent(None)
def create_field_filter(self, echo_nostr, echo_noarr):
def field_filter(val):
try:
# fields = val.__slots__
# field_types = val._slot_types
for f, t in zip(val.__slots__, val._slot_types):
if echo_noarr and '[' in t:
continue
elif echo_nostr and 'string' in t:
continue
yield f
except:
pass
return field_filter
def on_no_str_checkbox_toggled(self, state):
self.field_filter_fn = self.create_field_filter(
state, self.no_arr_checkbox.isChecked())
def on_no_arr_checkbox_toggled(self, state):
self.field_filter_fn = self.create_field_filter(
self.no_str_checkbox.isChecked(), state)
def combobox_reduce_ch_activated(self, ch_txt):
try:
self.line_limit = int(ch_txt)
except ValueError:
try:
self.line_limit = float(ch_txt)
except ValueError:
self.combobox_reduce_ch.setEditText(str(self.line_limit))
def on_combobox_hz_activated(self, hz_txt):
try:
self.receiving_hz = int(hz_txt)
except ValueError:
try:
self.receiving_hz = float(hz_txt)
except ValueError:
self.combobox_displ_hz.setEditText(str(self.receiving_hz))
def on_combobox_count_activated(self, count_txt):
try:
self.max_displayed_msgs = int(count_txt)
self._blocks_in_msg = None
except ValueError:
self.combobox_msgs_count.setEditText(str(self.max_displayed_msgs))
def on_clear_btn_clicked(self):
self.display.clear()
with self.lock:
self.message_count = 0
self._scrapped_msgs = 0
del self.times[:]
def on_topic_control_btn_clicked(self):
try:
if self.sub is None and self.ssh_output_file is None:
if self.__msg_class:
self.sub = rospy.Subscriber(self.topic, self.__msg_class,
self._msg_handle)
else:
self._on_display_anchorClicked(QUrl(self._masteruri))
self.topic_control_button.setText('stop')
self.topic_control_button.setIcon(
QIcon(':/icons/deleket_deviantart_stop.png'))
else:
if self.sub is not None:
self.sub.unregister()
self.sub = None
elif self.ssh_output_file is not None:
self.ssh_output_file.close()
self.ssh_error_file.close()
self.ssh_output_file = None
self.topic_control_button.setText('play')
self.topic_control_button.setIcon(
QIcon(':/icons/deleket_deviantart_play.png'))
self.no_str_checkbox.setEnabled(True)
self.no_arr_checkbox.setEnabled(True)
except Exception as e:
rospy.logwarn('Error while stop/play echo for topic %s: %s' %
(self.topic, e))
def _msg_handle(self, data):
self.msg_signal.emit(data,
(data._connection_header['latching'] != '0'))
def _append_message(self, msg, latched):
'''
Adds a label to the dialog's layout and shows the given text.
@param msg: the text to add to the dialog
@type msg: message object
'''
current_time = time.time()
self._count_messages(current_time)
# skip messages, if they are received often then MESSAGE_HZ_LIMIT
if self._last_received_ts != 0 and self.receiving_hz != 0:
if not latched and current_time - self._last_received_ts < 1.0 / self.receiving_hz:
self._scrapped_msgs += 1
self._scrapped_msgs_sl += 1
return
self._last_received_ts = current_time
if not self.show_only_rate:
# convert message to string and reduce line width to current limit
msg = message.strify_message(msg,
field_filter=self.field_filter_fn)
if isinstance(msg, tuple):
msg = msg[0]
msg = self._trim_width(msg)
msg = msg.replace('<', '<').replace('>', '>')
# create a notification about scrapped messages
if self._scrapped_msgs_sl > 0:
txt = '<pre style="color:red; font-family:Fixedsys,Courier,monospace; padding:10px;">scrapped %s message because of Hz-settings</pre>' % self._scrapped_msgs_sl
self.display.append(txt)
self._scrapped_msgs_sl = 0
txt = '<pre style="background-color:#FFFCCC; font-family:Fixedsys,Courier; padding:10px;">---------- %s --------------------\n%s</pre>' % (
datetime.now().strftime("%d.%m.%Y %H:%M:%S.%f"), msg)
# set the count of the displayed messages on receiving the first message
self._update_max_msg_count(txt)
self.display.append(txt)
self._print_status()
def _count_messages(self, ts=time.time()):
'''
Counts the received messages. Call this method only on receive message.
'''
current_time = ts
with self.lock:
# time reset
if self.msg_t0 < 0 or self.msg_t0 > current_time:
self.msg_t0 = current_time
self.msg_tn = current_time
self.times = []
else:
self.times.append(current_time - self.msg_tn)
self.msg_tn = current_time
# keep only statistics for the last 5000 messages so as not to run out of memory
if len(self.times) > self.STATISTIC_QUEUE_LEN:
self.times.pop(0)
self.message_count += 1
def _trim_width(self, msg):
'''
reduce line width to current limit
:param msg: the message
:type msg: str
:return: trimmed message
'''
result = msg
if self.line_limit != 0:
a = ''
for l in msg.splitlines():
a = a + (l if len(l) <= self.line_limit else
l[0:self.line_limit - 3] + '...') + '\n'
result = a
return result
def _update_max_msg_count(self, txt):
'''
set the count of the displayed messages on receiving the first message
:param txt: text of the message, which will be added to the document
:type txt: str
'''
if self._blocks_in_msg is None:
td = QTextDocument(txt)
self._blocks_in_msg = td.blockCount()
self.display.document().setMaximumBlockCount(
self._blocks_in_msg * self.max_displayed_msgs)
def _on_calc_hz(self):
if rospy.is_shutdown():
self.close()
return
if self.message_count == self.last_printed_count:
return
with self.lock:
# the code from ROS rostopic
n = len(self.times)
if n < 2:
return
mean = sum(self.times) / n
rate = 1. / mean if mean > 0. else 0
# std dev
std_dev = math.sqrt(sum((x - mean)**2 for x in self.times) / n)
# min and max
max_delta = max(self.times)
min_delta = min(self.times)
self.last_printed_count = self.message_count
self._rate_message = "average rate: %.3f\tmin: %.3fs max: %.3fs std dev: %.5fs window: %s" % (
rate, min_delta, max_delta, std_dev, n + 1)
if self._scrapped_msgs > 0:
self._rate_message += " --- scrapped msgs: %s" % self._scrapped_msgs
self._print_status()
if self.show_only_rate:
self.display.append(self._rate_message)
def _print_status(self):
self.status_label.setText('%s messages %s' %
(self.message_count, self._rate_message))
def _append_text(self, text):
'''
Append echo text received through the SSH.
'''
with self.lock:
self._current_msg += text
if self._current_msg.find('---') != -1:
messages = self._current_msg.split('---')
for m in messages[:-1]:
current_time = time.time()
self._count_messages(current_time)
# limit the displayed text width
m = self._trim_width(m)
txt = '<pre style="background-color:#FFFCCC; font-family:Fixedsys,Courier; padding:10px;">---------- %s --------------------\n%s</pre>' % (
datetime.now().strftime("%d.%m.%Y %H:%M:%S.%f"), m)
# set the count of the displayed messages on receiving the first message
self._update_max_msg_count(txt)
self.display.append(txt)
self._current_msg = messages[-1]
self._print_status()
def _append_error_text(self, text):
'''
Append error text received through the SSH.
'''
with self.lock:
self._current_errmsg += text
if self._current_errmsg.find('\n') != -1:
messages = self._current_errmsg.split('\n')
for m in messages[:-1]:
txt = '<pre style="color:red; font-family:Fixedsys,Courier,monospace; padding:10px;">%s</pre>' % m
self.display.append(txt)
self._current_errmsg = messages[-1]
def _append_text_hz(self, text):
'''
Append text received through the SSH for hz view.
'''
with self.lock:
self._current_msg += text
if self._current_msg.find('\n') != -1:
messages = self._current_msg.split('\n')
for m in messages[:-1]:
txt = '<div style="font-family:Fixedsys,Courier;">%s</div>' % (
m)
self.display.append(txt)
self._current_msg = messages[-1]
def _on_display_anchorClicked(self, url, user=None, pw=None):
try:
ok = False
if self.show_only_rate:
self.ssh_input_file, self.ssh_output_file, self.ssh_error_file, ok = nm.ssh(
).ssh_exec(url.host(), ['rostopic hz %s' % (self.topic)],
user,
pw,
auto_pw_request=True,
get_pty=True)
self.status_label.setText('connected to %s over SSH' %
url.host())
else:
self.combobox_displ_hz.setEnabled(False)
nostr = '--nostr' if self.no_str_checkbox.isChecked() else ''
noarr = '--noarr' if self.no_arr_checkbox.isChecked() else ''
self.ssh_input_file, self.ssh_output_file, self.ssh_error_file, ok = nm.ssh(
).ssh_exec(
url.host(),
['rostopic echo %s %s %s' % (nostr, noarr, self.topic)],
user,
pw,
auto_pw_request=True,
get_pty=True)
if ok:
self.display.clear()
target = self._read_output_hz if self.show_only_rate else self._read_output
thread = threading.Thread(target=target,
args=((self.ssh_output_file, )))
thread.setDaemon(True)
thread.start()
thread = threading.Thread(target=self._read_error,
args=((self.ssh_error_file, )))
thread.setDaemon(True)
thread.start()
elif self.ssh_output_file:
self.ssh_output_file.close()
self.ssh_error_file.close()
except Exception as e:
self._append_error_text('%s\n' % e)
# import traceback
# print traceback.format_exc()
def _read_output_hz(self, output_file):
try:
while not output_file.closed:
text = output_file.read(1)
if text:
self.text_hz_signal.emit(text)
except:
pass
# import traceback
# print traceback.format_exc()
def _read_output(self, output_file):
while not output_file.closed:
text = output_file.read(1)
if text:
self.text_signal.emit(text)
def _read_error(self, error_file):
try:
while not error_file.closed:
text = error_file.read(1)
if text:
self.text_error_signal.emit(text)
except:
pass
class Editor(QMainWindow):
'''
Creates a dialog to edit a launch file.
'''
finished_signal = Signal(list)
'''
finished_signal has as parameter the filenames of the initialization and is emitted, if this
dialog was closed.
'''
def __init__(self, filenames, search_text='', parent=None):
'''
@param filenames: a list with filenames. The last one will be activated.
@type filenames: C{[str, ...]}
@param search_text: if not empty, searches in new document for first occurrence of the given text
@type search_text: C{str} (Default: C{Empty String})
'''
QMainWindow.__init__(self, parent)
self.setObjectName(' - '.join(['Editor', str(filenames)]))
self.setAttribute(Qt.WA_DeleteOnClose, True)
self.setWindowFlags(Qt.Window)
self.mIcon = QIcon(":/icons/crystal_clear_edit_launch.png")
self._error_icon = QIcon(":/icons/crystal_clear_warning.png")
self._empty_icon = QIcon()
self.setWindowIcon(self.mIcon)
window_title = "ROSLaunch Editor"
if filenames:
window_title = self.__getTabName(filenames[0])
self.setWindowTitle(window_title)
self.init_filenames = list(filenames)
self._search_thread = None
# list with all open files
self.files = []
# create tabs for files
self.main_widget = QWidget(self)
self.verticalLayout = QVBoxLayout(self.main_widget)
self.verticalLayout.setContentsMargins(0, 0, 0, 0)
self.verticalLayout.setSpacing(1)
self.verticalLayout.setObjectName("verticalLayout")
self.tabWidget = EditorTabWidget(self)
self.tabWidget.setTabPosition(QTabWidget.North)
self.tabWidget.setDocumentMode(True)
self.tabWidget.setTabsClosable(True)
self.tabWidget.setMovable(False)
self.tabWidget.setObjectName("tabWidget")
self.tabWidget.tabCloseRequested.connect(self.on_close_tab)
self.verticalLayout.addWidget(self.tabWidget)
self.buttons = self._create_buttons()
self.verticalLayout.addWidget(self.buttons)
self.setCentralWidget(self.main_widget)
self.find_dialog = TextSearchFrame(self.tabWidget, self)
self.find_dialog.search_result_signal.connect(self.on_search_result)
self.find_dialog.replace_signal.connect(self.on_replace)
self.addDockWidget(Qt.RightDockWidgetArea, self.find_dialog)
# open the files
for f in filenames:
if f:
self.on_load_request(os.path.normpath(f), search_text)
self.readSettings()
self.find_dialog.setVisible(False)
# def __del__(self):
# print "******** destroy", self.objectName()
def _create_buttons(self):
# create the buttons line
self.buttons = QWidget(self)
self.horizontalLayout = QHBoxLayout(self.buttons)
self.horizontalLayout.setContentsMargins(4, 0, 4, 0)
self.horizontalLayout.setObjectName("horizontalLayout")
# add the search button
self.searchButton = QPushButton(self)
self.searchButton.setObjectName("searchButton")
# self.searchButton.clicked.connect(self.on_shortcut_find)
self.searchButton.toggled.connect(self.on_toggled_find)
self.searchButton.setText(self._translate("&Find"))
self.searchButton.setToolTip('Open a search dialog (Ctrl+F)')
self.searchButton.setFlat(True)
self.searchButton.setCheckable(True)
self.horizontalLayout.addWidget(self.searchButton)
# add the replace button
self.replaceButton = QPushButton(self)
self.replaceButton.setObjectName("replaceButton")
# self.replaceButton.clicked.connect(self.on_shortcut_replace)
self.replaceButton.toggled.connect(self.on_toggled_replace)
self.replaceButton.setText(self._translate("&Replace"))
self.replaceButton.setToolTip('Open a search&replace dialog (Ctrl+R)')
self.replaceButton.setFlat(True)
self.replaceButton.setCheckable(True)
self.horizontalLayout.addWidget(self.replaceButton)
# add the goto button
self.gotoButton = QPushButton(self)
self.gotoButton.setObjectName("gotoButton")
self.gotoButton.clicked.connect(self.on_shortcut_goto)
self.gotoButton.setText(self._translate("&Goto line"))
self.gotoButton.setShortcut("Ctrl+G")
self.gotoButton.setToolTip('Open a goto dialog (Ctrl+G)')
self.gotoButton.setFlat(True)
self.horizontalLayout.addWidget(self.gotoButton)
# add a tag button
self.tagButton = self._create_tag_button(self)
self.horizontalLayout.addWidget(self.tagButton)
# add spacer
spacerItem = QSpacerItem(515, 20, QSizePolicy.Expanding, QSizePolicy.Minimum)
self.horizontalLayout.addItem(spacerItem)
# add line number label
self.pos_label = QLabel()
self.horizontalLayout.addWidget(self.pos_label)
# add spacer
spacerItem = QSpacerItem(515, 20, QSizePolicy.Expanding, QSizePolicy.Minimum)
self.horizontalLayout.addItem(spacerItem)
# add save button
self.saveButton = QPushButton(self)
self.saveButton.setObjectName("saveButton")
self.saveButton.clicked.connect(self.on_saveButton_clicked)
self.saveButton.setText(self._translate("&Save"))
self.saveButton.setShortcut("Ctrl+S")
self.saveButton.setToolTip('Save the changes to the file (Ctrl+S)')
self.saveButton.setFlat(True)
self.horizontalLayout.addWidget(self.saveButton)
return self.buttons
def keyPressEvent(self, event):
'''
Enable the shortcats for search and replace
'''
if event.key() == Qt.Key_Escape:
self.reject()
elif event.modifiers() == Qt.ControlModifier and event.key() == Qt.Key_F:
if self.tabWidget.currentWidget().hasFocus():
if not self.searchButton.isChecked():
self.searchButton.setChecked(True)
else:
self.on_toggled_find(True)
else:
self.searchButton.setChecked(not self.searchButton.isChecked())
elif event.modifiers() == Qt.ControlModifier and event.key() == Qt.Key_R:
if self.tabWidget.currentWidget().hasFocus():
if not self.replaceButton.isChecked():
self.replaceButton.setChecked(True)
else:
self.on_toggled_replace(True)
else:
self.replaceButton.setChecked(not self.replaceButton.isChecked())
else:
event.accept()
QMainWindow.keyPressEvent(self, event)
def _translate(self, text):
if hasattr(QApplication, "UnicodeUTF8"):
return QApplication.translate("Editor", text, None, QApplication.UnicodeUTF8)
else:
return QApplication.translate("Editor", text, None)
def readSettings(self):
if nm.settings().store_geometry:
settings = nm.settings().qsettings(nm.settings().CFG_GUI_FILE)
settings.beginGroup("editor")
maximized = settings.value("maximized", 'false') == 'true'
if maximized:
self.showMaximized()
else:
self.resize(settings.value("size", QSize(800, 640)))
self.move(settings.value("pos", QPoint(0, 0)))
try:
self.restoreState(settings.value("window_state"))
except:
import traceback
print traceback.format_exc()
settings.endGroup()
def storeSetting(self):
if nm.settings().store_geometry:
settings = nm.settings().qsettings(nm.settings().CFG_GUI_FILE)
settings.beginGroup("editor")
settings.setValue("size", self.size())
settings.setValue("pos", self.pos())
settings.setValue("maximized", self.isMaximized())
settings.setValue("window_state", self.saveState())
settings.endGroup()
def on_load_request(self, filename, search_text=''):
'''
Loads a file in a new tab or focus the tab, if the file is already open.
@param filename: the path to file
@type filename: C{str}
@param search_text: if not empty, searches in new document for first occurrence of the given text
@type search_text: C{str} (Default: C{Empty String})
'''
if not filename:
return
self.tabWidget.setUpdatesEnabled(False)
try:
if filename not in self.files:
tab_name = self.__getTabName(filename)
editor = TextEdit(filename, self.tabWidget)
linenumber_editor = LineNumberWidget(editor)
tab_index = self.tabWidget.addTab(linenumber_editor, tab_name)
self.files.append(filename)
editor.setCurrentPath(os.path.basename(filename))
editor.load_request_signal.connect(self.on_load_request)
editor.document().modificationChanged.connect(self.on_editor_modificationChanged)
editor.cursorPositionChanged.connect(self.on_editor_positionChanged)
editor.setFocus(Qt.OtherFocusReason)
# editor.textChanged.connect(self.on_text_changed)
editor.undoAvailable.connect(self.on_text_changed)
self.tabWidget.setCurrentIndex(tab_index)
# self.find_dialog.set_search_path(filename)
else:
for i in range(self.tabWidget.count()):
if self.tabWidget.widget(i).filename == filename:
self.tabWidget.setCurrentIndex(i)
break
except:
import traceback
rospy.logwarn("Error while open %s: %s", filename, traceback.format_exc(1))
self.tabWidget.setUpdatesEnabled(True)
if search_text:
try:
self._search_thread.stop()
self._search_thread = None
except:
pass
self._search_thread = TextSearchThread(search_text, filename, path_text=self.tabWidget.widget(0).document().toPlainText(), recursive=True)
self._search_thread.search_result_signal.connect(self.on_search_result_on_open)
self._search_thread.start()
def on_text_changed(self, value=""):
if self.tabWidget.currentWidget().hasFocus():
self.find_dialog.file_changed(self.tabWidget.currentWidget().filename)
def on_close_tab(self, tab_index):
'''
Signal handling to close single tabs.
@param tab_index: tab index to close
@type tab_index: C{int}
'''
try:
doremove = True
w = self.tabWidget.widget(tab_index)
if w.document().isModified():
name = self.__getTabName(w.filename)
result = QMessageBox.question(self, "Unsaved Changes", '\n\n'.join(["Save the file before closing?", name]), QMessageBox.Yes | QMessageBox.No | QMessageBox.Cancel)
if result == QMessageBox.Yes:
self.tabWidget.currentWidget().save()
elif result == QMessageBox.No:
pass
else:
doremove = False
if doremove:
# remove the indexed files
if w.filename in self.files:
self.files.remove(w.filename)
# close tab
self.tabWidget.removeTab(tab_index)
# close editor, if no tabs are open
if not self.tabWidget.count():
self.close()
except:
import traceback
rospy.logwarn("Error while close tab %s: %s", str(tab_index), traceback.format_exc(1))
def reject(self):
if self.find_dialog.isVisible():
self.searchButton.setChecked(not self.searchButton.isChecked())
else:
self.close()
def closeEvent(self, event):
'''
Test the open files for changes and save this if needed.
'''
changed = []
# get the names of all changed files
for i in range(self.tabWidget.count()):
w = self.tabWidget.widget(i)
if w.document().isModified():
changed.append(self.__getTabName(w.filename))
if changed:
# ask the user for save changes
if self.isHidden():
buttons = QMessageBox.Yes | QMessageBox.No
else:
buttons = QMessageBox.Yes | QMessageBox.No | QMessageBox.Cancel
result = QMessageBox.question(self, "Unsaved Changes", '\n\n'.join(["Save the file before closing?", '\n'.join(changed)]), buttons)
if result == QMessageBox.Yes:
for i in range(self.tabWidget.count()):
w = self.tabWidget.widget(i).save()
event.accept()
elif result == QMessageBox.No:
event.accept()
else:
event.ignore()
else:
event.accept()
if event.isAccepted():
self.storeSetting()
self.finished_signal.emit(self.init_filenames)
def on_editor_modificationChanged(self, value=None):
'''
If the content was changed, a '*' will be shown in the tab name.
'''
tab_name = self.__getTabName(self.tabWidget.currentWidget().filename)
if (self.tabWidget.currentWidget().document().isModified()) or not QFileInfo(self.tabWidget.currentWidget().filename).exists():
tab_name = ''.join(['*', tab_name])
self.tabWidget.setTabText(self.tabWidget.currentIndex(), tab_name)
def on_editor_positionChanged(self):
'''
Shows the number of the line and column in a label.
'''
cursor = self.tabWidget.currentWidget().textCursor()
self.pos_label.setText(':%s:%s #%s' % (cursor.blockNumber() + 1, cursor.columnNumber(), cursor.position()))
def __getTabName(self, lfile):
base = os.path.basename(lfile).replace('.launch', '')
(package, _) = package_name(os.path.dirname(lfile))
return '%s [%s]' % (base, package)
##############################################################################
# HANDLER for buttons
##############################################################################
def on_saveButton_clicked(self):
'''
Saves the current document. This method is called if the C{save button}
was clicked.
'''
saved, errors, msg = self.tabWidget.currentWidget().save(True)
if errors:
QMessageBox.critical(self, "Error", msg)
self.tabWidget.setTabIcon(self.tabWidget.currentIndex(), self._error_icon)
self.tabWidget.setTabToolTip(self.tabWidget.currentIndex(), msg)
elif saved:
self.tabWidget.setTabIcon(self.tabWidget.currentIndex(), self._empty_icon)
self.tabWidget.setTabToolTip(self.tabWidget.currentIndex(), '')
def on_shortcut_find(self):
pass
def on_toggled_find(self, value):
'''
Shows the search frame
'''
if value:
self.find_dialog.enable()
else:
self.replaceButton.setChecked(False)
self.find_dialog.setVisible(False)
self.tabWidget.currentWidget().setFocus()
def on_toggled_replace(self, value):
'''
Shows the replace lineedit in the search frame
'''
if value:
self.searchButton.setChecked(True)
self.find_dialog.set_replace_visible(value)
def on_shortcut_goto(self):
'''
Opens a C{goto} dialog.
'''
value = 1
ok = False
try:
value, ok = QInputDialog.getInt(self, "Goto", self.tr("Line number:"),
QLineEdit.Normal, minValue=1, step=1)
except:
value, ok = QInputDialog.getInt(self, "Goto", self.tr("Line number:"),
QLineEdit.Normal, min=1, step=1)
if ok:
if value > self.tabWidget.currentWidget().document().blockCount():
value = self.tabWidget.currentWidget().document().blockCount()
curpos = self.tabWidget.currentWidget().textCursor().blockNumber() + 1
while curpos != value:
mov = QTextCursor.NextBlock if curpos < value else QTextCursor.PreviousBlock
self.tabWidget.currentWidget().moveCursor(mov)
curpos = self.tabWidget.currentWidget().textCursor().blockNumber() + 1
self.tabWidget.currentWidget().setFocus(Qt.ActiveWindowFocusReason)
##############################################################################
# SLOTS for search dialog
##############################################################################
def on_search_result(self, search_text, found, path, index):
'''
A slot to handle a found text. It goes to the position in the text and select
the searched text. On new file it will be open.
:param search_text: the searched text
:type search_text: str
:param found: the text was found or not
:type found: bool
:param path: the path of the file the text was found
:type path: str
:param index: the position in the text
:type index: int
'''
if found:
if self.tabWidget.currentWidget().filename != path:
focus_widget = QApplication.focusWidget()
self.on_load_request(path)
focus_widget.setFocus()
cursor = self.tabWidget.currentWidget().textCursor()
cursor.setPosition(index, QTextCursor.MoveAnchor)
cursor.movePosition(QTextCursor.NextCharacter, QTextCursor.KeepAnchor, len(search_text))
self.tabWidget.currentWidget().setTextCursor(cursor)
def on_search_result_on_open(self, search_text, found, path, index):
'''
Like on_search_result, but skips the text in comments.
'''
if found:
if self.tabWidget.currentWidget().filename != path:
focus_widget = QApplication.focusWidget()
self.on_load_request(path)
focus_widget.setFocus()
comment_start = self.tabWidget.currentWidget().document().find('<!--', index, QTextDocument.FindBackward)
if not comment_start.isNull():
comment_end = self.tabWidget.currentWidget().document().find('-->', comment_start)
if not comment_end.isNull() and comment_end.position() > index + len(search_text):
# commented -> retrun
return
self.on_search_result(search_text, found, path, index)
def on_replace(self, search_text, path, index, replaced_text):
'''
A slot to handle a text replacement of the TextSearchFrame.
:param search_text: the searched text
:type search_text: str
:param path: the path of the file the text was found
:type path: str
:param index: the position in the text
:type index: int
:param replaced_text: the new text
:type replaced_text: str
'''
cursor = self.tabWidget.currentWidget().textCursor()
if cursor.selectedText() == search_text:
cursor.insertText(replaced_text)
##############################################################################
# LAUNCH TAG insertion
##############################################################################
def _create_tag_button(self, parent=None):
btn = QPushButton(parent)
btn.setObjectName("tagButton")
btn.setText(self._translate("Add &tag"))
btn.setShortcut("Ctrl+T")
btn.setToolTip('Adds a ROS launch tag to launch file (Ctrl+T)')
btn.setMenu(self._create_tag_menu(btn))
btn.setFlat(True)
return btn
def _create_tag_menu(self, parent=None):
# creates a tag menu
tag_menu = QMenu("ROS Tags", parent)
# group tag
add_group_tag_action = QAction("<group>", self, statusTip="", triggered=self._on_add_group_tag)
add_group_tag_action.setShortcuts(QKeySequence("Ctrl+Shift+g"))
tag_menu.addAction(add_group_tag_action)
# node tag
add_node_tag_action = QAction("<node>", self, statusTip="", triggered=self._on_add_node_tag)
add_node_tag_action.setShortcuts(QKeySequence("Ctrl+Shift+n"))
tag_menu.addAction(add_node_tag_action)
# node tag with all attributes
add_node_tag_all_action = QAction("<node all>", self, statusTip="", triggered=self._on_add_node_tag_all)
tag_menu.addAction(add_node_tag_all_action)
# include tag with all attributes
add_include_tag_all_action = QAction("<include>", self, statusTip="", triggered=self._on_add_include_tag_all)
add_include_tag_all_action.setShortcuts(QKeySequence("Ctrl+Shift+i"))
tag_menu.addAction(add_include_tag_all_action)
# remap
add_remap_tag_action = QAction("<remap>", self, statusTip="", triggered=self._on_add_remap_tag)
add_remap_tag_action.setShortcuts(QKeySequence("Ctrl+Shift+r"))
tag_menu.addAction(add_remap_tag_action)
# env tag
add_env_tag_action = QAction("<env>", self, statusTip="", triggered=self._on_add_env_tag)
tag_menu.addAction(add_env_tag_action)
# param tag
add_param_tag_action = QAction("<param>", self, statusTip="", triggered=self._on_add_param_tag)
add_param_tag_action.setShortcuts(QKeySequence("Ctrl+Shift+p"))
tag_menu.addAction(add_param_tag_action)
# param capability group tag
add_param_cap_group_tag_action = QAction("<param capability group>", self, statusTip="", triggered=self._on_add_param_cap_group_tag)
add_param_cap_group_tag_action.setShortcuts(QKeySequence("Ctrl+Alt+p"))
tag_menu.addAction(add_param_cap_group_tag_action)
# param tag with all attributes
add_param_tag_all_action = QAction("<param all>", self, statusTip="", triggered=self._on_add_param_tag_all)
tag_menu.addAction(add_param_tag_all_action)
# rosparam tag with all attributes
add_rosparam_tag_all_action = QAction("<rosparam>", self, statusTip="", triggered=self._on_add_rosparam_tag_all)
tag_menu.addAction(add_rosparam_tag_all_action)
# arg tag with default definition
add_arg_tag_default_action = QAction("<arg default>", self, statusTip="", triggered=self._on_add_arg_tag_default)
add_arg_tag_default_action.setShortcuts(QKeySequence("Ctrl+Shift+a"))
tag_menu.addAction(add_arg_tag_default_action)
# arg tag with value definition
add_arg_tag_value_action = QAction("<arg value>", self, statusTip="", triggered=self._on_add_arg_tag_value)
add_arg_tag_value_action.setShortcuts(QKeySequence("Ctrl+Alt+a"))
tag_menu.addAction(add_arg_tag_value_action)
# test tag
add_test_tag_action = QAction("<test>", self, statusTip="", triggered=self._on_add_test_tag)
add_test_tag_action.setShortcuts(QKeySequence("Ctrl+Alt+t"))
tag_menu.addAction(add_test_tag_action)
# test tag with all attributes
add_test_tag_all_action = QAction("<test all>", self, statusTip="", triggered=self._on_add_test_tag_all)
tag_menu.addAction(add_test_tag_all_action)
return tag_menu
def _insert_text(self, text):
cursor = self.tabWidget.currentWidget().textCursor()
if not cursor.isNull():
col = cursor.columnNumber()
spaces = ''.join([' ' for _ in range(col)])
cursor.insertText(text.replace('\n', '\n%s' % spaces))
self.tabWidget.currentWidget().setFocus(Qt.OtherFocusReason)
def _on_add_group_tag(self):
self._insert_text('<group ns="namespace" clear_params="true|false">\n'
'</group>')
def _on_add_node_tag(self):
dia = PackageDialog()
if dia.exec_():
self._insert_text('<node name="%s" pkg="%s" type="%s">\n'
'</node>' % (dia.binary, dia.package, dia.binary))
def _on_add_node_tag_all(self):
dia = PackageDialog()
if dia.exec_():
self._insert_text('<node name="%s" pkg="%s" type="%s"\n'
' args="arg1" machine="machine-name"\n'
' respawn="true" required="true"\n'
' ns="foo" clear_params="true|false"\n'
' output="log|screen" cwd="ROS_HOME|node"\n'
' launch-prefix="prefix arguments">\n'
'</node>' % (dia.binary, dia.package, dia.binary))
def _on_add_include_tag_all(self):
self._insert_text('<include file="$(find pkg-name)/path/filename.xml"\n'
' ns="foo" clear_params="true|false">\n'
'</include>')
def _on_add_remap_tag(self):
self._insert_text('<remap from="original" to="new"/>')
def _on_add_env_tag(self):
self._insert_text('<env name="variable" value="value"/>')
def _on_add_param_tag(self):
self._insert_text('<param name="ns_name" value="value" />')
def _on_add_param_cap_group_tag(self):
self._insert_text('<param name="capability_group" value="demo" />')
def _on_add_param_tag_all(self):
self._insert_text('<param name="ns_name" value="value"\n'
' type="str|int|double|bool"\n'
' textfile="$(find pkg-name)/path/file.txt"\n'
' binfile="$(find pkg-name)/path/file"\n'
' command="$(find pkg-name)/exe \'$(find pkg-name)/arg.txt\'">\n'
'</param>')
def _on_add_rosparam_tag_all(self):
self._insert_text('<rosparam param="param-name"\n'
' file="$(find pkg-name)/path/foo.yaml"\n'
' command="load|dump|delete"\n'
' ns="namespace">\n'
'</rosparam>')
def _on_add_arg_tag_default(self):
self._insert_text('<arg name="foo" default="1" />')
def _on_add_arg_tag_value(self):
self._insert_text('<arg name="foo" value="bar" />')
def _on_add_test_tag(self):
dia = PackageDialog()
if dia.exec_():
self._insert_text('<test name="%s" pkg="%s" type="%s" test-name="test_%s">\n'
'</test>' % (dia.binary, dia.package, dia.binary, dia.binary))
def _on_add_test_tag_all(self):
dia = PackageDialog()
if dia.exec_():
self._insert_text('<test name="%s" pkg="%s" type="%s" test-name="test_%s">\n'
' args="arg1" time-limit="60.0"\n'
' ns="foo" clear_params="true|false"\n'
' cwd="ROS_HOME|node" retry="0"\n'
' launch-prefix="prefix arguments">\n'
'</test>' % (dia.binary, dia.package, dia.binary, dia.binary))
def create_controller_ui(self):
self.controllers = []
if not self.load_robot_model():
# if no groups config is on the parameter server, request the list from controller manager
self.load_controllers()
robot = URDF.from_parameter_server()
joint_list = {}
for ndx, jnt in enumerate(robot.joints):
joint_list[jnt.name] = ndx
for controller in self.controllers:
frame = QFrame()
frame.setFrameShape(QFrame.StyledPanel)
frame.setFrameShadow(QFrame.Raised)
vbox = QVBoxLayout()
label = QLabel()
label.setText(controller.label)
vbox.addWidget(label)
controller.snap_to_ghost_button = QPushButton("SnapGhost")
controller.snap_to_ghost_button.pressed.connect(controller.on_snap_ghost_pressed)
vbox.addWidget(controller.snap_to_ghost_button)
controller.snap_to_current_button = QPushButton("SnapCurrent")
controller.snap_to_current_button.pressed.connect(controller.on_snap_current_pressed)
vbox.addWidget(controller.snap_to_current_button)
controller.apply_to_robot_button = QPushButton("ApplyRobot")
controller.apply_to_robot_button.pressed.connect(controller.on_apply_robot_pressed)
vbox.addWidget(controller.apply_to_robot_button)
# Removed because it is hardcoded
# controller.save_joints_to_file_button = QPushButton("SaveJoints")
# controller.save_joints_to_file_button.pressed.connect(controller.on_save_joints_pressed)
# vbox.addWidget(controller.save_joints_to_file_button)
controller.undo_last_action_button = QPushButton("Undo Last")
controller.undo_last_action_button.pressed.connect(controller.on_undo_pressed)
vbox.addWidget(controller.undo_last_action_button)
print 'Loading limits for controller:', controller.topic
for joint in controller.joints:
label = QLabel()
label.setText(joint.name)
vbox.addWidget(label)
try:
robot_joint = robot.joints[joint_list[joint.name]]
except KeyError:
print 'No limits found for', joint.name
limit_lower = -1.0
limit_upper = 1
else:
limit_lower = robot_joint.limit.lower
limit_upper = robot_joint.limit.upper
print " ", joint.name, " limits(", limit_lower, ", ", limit_upper, ") num"
joint.slider = QSlider(Qt.Horizontal)
joint.slider.setRange(int(limit_lower * 10000.0), int(limit_upper * 10000.0))
joint.slider.setValue(int(limit_lower * 10000.0))
joint.slider.setSingleStep((limit_upper - limit_lower) / 20.0)
joint.slider.valueChanged.connect(joint.on_slider_moved)
vbox.addWidget(joint.slider)
joint.progress_bar = QProgressBar()
joint.progress_bar.setRange(int(limit_lower * 10000.0),
int(limit_upper * 10000.0))
joint.progress_bar.setValue(int(limit_lower * 10000.0))
vbox.addWidget(joint.progress_bar)
vbox.addStretch()
frame.setLayout(vbox)
self.widget.addWidget(frame)
class GroupWidget(QWidget):
'''
(Isaac's guess as of 12/13/2012)
This class bonds multiple Editor instances that are associated with
a single node as a group.
'''
# public signal
sig_node_disabled_selected = Signal(str)
def __init__(self, updater, config, nodename):
'''
:param config:
:type config: Dictionary? defined in dynamic_reconfigure.client.Client
:type nodename: str
'''
#TODO figure out what data type 'config' is. It is afterall returned
# from dynamic_reconfigure.client.get_parameter_descriptions()
# ros.org/doc/api/dynamic_reconfigure/html/dynamic_reconfigure.client-pysrc.html#Client
super(GroupWidget, self).__init__()
self.state = config['state']
self.name = config['name']
self._toplevel_treenode_name = nodename
# TODO: .ui file needs to be back into usage in later phase.
# ui_file = os.path.join(rp.get_path('rqt_reconfigure'),
# 'resource', 'singlenode_parameditor.ui')
# loadUi(ui_file, self)
verticalLayout = QVBoxLayout(self)
verticalLayout.setContentsMargins(QMargins(0, 0, 0, 0))
_widget_nodeheader = QWidget()
_h_layout_nodeheader = QHBoxLayout(_widget_nodeheader)
_h_layout_nodeheader.setContentsMargins(QMargins(0, 0, 0, 0))
self.nodename_qlabel = QLabel(self)
font = QFont('Trebuchet MS, Bold')
font.setUnderline(True)
font.setBold(True)
# Button to close a node.
_icon_disable_node = QIcon.fromTheme('window-close')
_bt_disable_node = QPushButton(_icon_disable_node, '', self)
_bt_disable_node.setToolTip('Hide this node')
_bt_disable_node_size = QSize(36, 24)
_bt_disable_node.setFixedSize(_bt_disable_node_size)
_bt_disable_node.pressed.connect(self._node_disable_bt_clicked)
_h_layout_nodeheader.addWidget(self.nodename_qlabel)
_h_layout_nodeheader.addWidget(_bt_disable_node)
self.nodename_qlabel.setAlignment(Qt.AlignCenter)
font.setPointSize(10)
self.nodename_qlabel.setFont(font)
grid_widget = QWidget(self)
self.grid = QFormLayout(grid_widget)
verticalLayout.addWidget(_widget_nodeheader)
verticalLayout.addWidget(grid_widget, 1)
# Again, these UI operation above needs to happen in .ui file.
self.tab_bar = None # Every group can have one tab bar
self.tab_bar_shown = False
self.updater = updater
self.editor_widgets = []
self._param_names = []
self._create_node_widgets(config)
rospy.logdebug('Groups node name={}'.format(nodename))
self.nodename_qlabel.setText(nodename)
# Labels should not stretch
#self.grid.setColumnStretch(1, 1)
#self.setLayout(self.grid)
def collect_paramnames(self, config):
pass
def _create_node_widgets(self, config):
'''
:type config: Dict?
'''
i_debug = 0
for param in config['parameters']:
begin = time.time() * 1000
editor_type = '(none)'
if param['edit_method']:
widget = EnumEditor(self.updater, param)
elif param['type'] in EDITOR_TYPES:
rospy.logdebug('GroupWidget i_debug=%d param type =%s',
i_debug, param['type'])
editor_type = EDITOR_TYPES[param['type']]
widget = eval(editor_type)(self.updater, param)
self.editor_widgets.append(widget)
self._param_names.append(param['name'])
rospy.logdebug('groups._create_node_widgets num editors=%d',
i_debug)
end = time.time() * 1000
time_elap = end - begin
rospy.logdebug('ParamG editor={} loop=#{} Time={}msec'.format(
editor_type, i_debug, time_elap))
i_debug += 1
for name, group in config['groups'].items():
if group['type'] == 'tab':
widget = TabGroup(self, self.updater, group)
elif group['type'] in _GROUP_TYPES.keys():
widget = eval(_GROUP_TYPES[group['type']])(self.updater, group)
self.editor_widgets.append(widget)
rospy.logdebug(
'groups._create_node_widgets ' +
#'num groups=%d' +
'name=%s',
name)
for i, ed in enumerate(self.editor_widgets):
ed.display(self.grid)
rospy.logdebug('GroupWdgt._create_node_widgets len(editor_widgets)=%d',
len(self.editor_widgets))
def display(self, grid, row):
# groups span across all columns
grid.addWidget(self, row, 0, 1, -1)
def update_group(self, config):
self.state = config['state']
# TODO: should use config.keys but this method doesnt exist
names = [name for name in config.items()]
for widget in self.editor_widgets:
if isinstance(widget, EditorWidget):
if widget.name in names:
widget.update_value(config[widget.name])
elif isinstance(widget, GroupWidget):
cfg = find_cfg(config, widget.name)
widget.update_group(cfg)
def close(self):
for w in self.editor_widgets:
w.close()
def get_treenode_names(self):
'''
:rtype: str[]
'''
return self._param_names
def _node_disable_bt_clicked(self):
rospy.logdebug('param_gs _node_disable_bt_clicked')
self.sig_node_disabled_selected.emit(self._toplevel_treenode_name)
class LevelSelectorPlugin(Plugin):
def __init__(self, context):
super(LevelSelectorPlugin, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('LevelSelectorPlugin')
# Create QWidget
self._widget = QWidget()
# self._widget.setFont(QFont("Times", 15, QFont.Bold))
self._button_layout = QVBoxLayout(self._widget)
self.buttons = []
self.text_label = QLabel("Waiting for MultiLevelMapData...",
self._widget)
self._button_layout.addWidget(self.text_label)
self._widget.setObjectName('LevelSelectorPluginUI')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
context.add_widget(self._widget)
self.connect(self._widget, SIGNAL("update_buttons"),
self.update_buttons)
self.connect(self._widget, SIGNAL("update_button_status"),
self.update_button_status)
# Subcribe to the multi level map data to get information about all the maps.
self.multimap_subscriber = rospy.Subscriber("map_metadata",
MultiLevelMapData,
self.process_multimap)
self.levels = []
self.current_level = None
# Subscribe to the current level we are on.
self.status_subscriber = None
# Create a service proxy to change the current level.
self.level_selector_proxy = rospy.ServiceProxy(
"level_mux/change_current_level", ChangeCurrentLevel)
self.level_selector_proxy.wait_for_service()
def process_multimap(self, msg):
self.levels = msg.levels
self._widget.emit(SIGNAL("update_buttons"))
def update_buttons(self):
self.clean()
for index, level in enumerate(self.levels):
self.text_label.setText("Choose Level: ")
button = QPushButton(level.level_id, self._widget)
button.clicked[bool].connect(self.handle_button)
button.setCheckable(True)
self._button_layout.addWidget(button)
self.buttons.append(button)
# Subscribe to the current level we are on.
if self.status_subscriber is None:
self.status_subscriber = rospy.Subscriber(
"level_mux/current_level", LevelMetaData,
self.process_level_status)
def update_button_status(self):
for index, level in enumerate(self.levels):
if self.current_level == level.level_id:
self.buttons[index].setChecked(True)
else:
self.buttons[index].setChecked(False)
def process_level_status(self, msg):
level_found = False
for level in self.levels:
if msg.level_id == level.level_id:
self.current_level = level.level_id
level_found = True
break
if not level_found:
self.current_level = None
self._widget.emit(SIGNAL("update_button_status"))
def handle_button(self):
source = self.sender()
if source.text() == self.current_level:
source.setChecked(True)
return
# Construct a identity pose. The level selector cannot be used to choose the initialpose, as it does not have
# the interface for specifying the position. The position should be specified via rviz.
origin_pose = PoseWithCovarianceStamped()
origin_pose.header.frame_id = frameIdFromLevelId(source.text())
origin_pose.pose.pose.orientation.w = 1 # Makes the origin quaternion valid.
origin_pose.pose.covariance[0] = 1.0
origin_pose.pose.covariance[7] = 1.0
origin_pose.pose.covariance[14] = 1.0
origin_pose.pose.covariance[21] = 1.0
origin_pose.pose.covariance[28] = 1.0
origin_pose.pose.covariance[35] = 1.0
# Don't actually publish the initial pose via the level selector. It doesn't know any better.
self.level_selector_proxy(source.text(), False, origin_pose)
def clean(self):
while self._button_layout.count():
item = self._button_layout.takeAt(0)
item.widget().deleteLater()
def save_settings(self, plugin_settings, instance_settings):
pass
def restore_settings(self, plugin_settings, instance_settings):
pass
class SelectDialog(QDialog):
'''
This dialog creates an input mask for a string list and return selected entries.
'''
def __init__(self,
items=list(),
buttons=QDialogButtonBox.Cancel | QDialogButtonBox.Ok,
exclusive=False,
preselect_all=False,
title='',
description='',
icon='',
parent=None,
select_if_single=True,
checkitem1='',
checkitem2='',
closein=0):
'''
Creates an input dialog.
@param items: a list with strings
@type items: C{list()}
'''
QDialog.__init__(self, parent=parent)
self.setObjectName(' - '.join(['SelectDialog', utf8(items)]))
self.verticalLayout = QVBoxLayout(self)
self.verticalLayout.setObjectName("verticalLayout")
self.verticalLayout.setContentsMargins(1, 1, 1, 1)
# add filter row
self.filter_frame = QFrame(self)
filterLayout = QHBoxLayout(self.filter_frame)
filterLayout.setContentsMargins(1, 1, 1, 1)
label = QLabel("Filter:", self.filter_frame)
self.filter_field = EnchancedLineEdit(self.filter_frame)
filterLayout.addWidget(label)
filterLayout.addWidget(self.filter_field)
self.filter_field.textChanged.connect(self._on_filter_changed)
self.verticalLayout.addWidget(self.filter_frame)
if description:
self.description_frame = QFrame(self)
descriptionLayout = QHBoxLayout(self.description_frame)
# descriptionLayout.setContentsMargins(1, 1, 1, 1)
if icon:
self.icon_label = QLabel(self.description_frame)
self.icon_label.setSizePolicy(QSizePolicy.Fixed,
QSizePolicy.Fixed)
self.icon_label.setPixmap(
QPixmap(icon).scaled(30, 30, Qt.KeepAspectRatio))
descriptionLayout.addWidget(self.icon_label)
self.description_label = QLabel(self.description_frame)
self.description_label.setWordWrap(True)
self.description_label.setText(description)
descriptionLayout.addWidget(self.description_label)
self.verticalLayout.addWidget(self.description_frame)
# create area for the parameter
self.content = MainBox(self)
if items:
self.scroll_area = QScrollArea(self)
self.scroll_area.setFocusPolicy(Qt.NoFocus)
self.scroll_area.setObjectName("scroll_area")
self.scroll_area.setWidgetResizable(True)
self.scroll_area.setWidget(self.content)
self.verticalLayout.addWidget(self.scroll_area)
self.checkitem1 = checkitem1
self.checkitem1_result = False
self.checkitem2 = checkitem2
self.checkitem2_result = False
# add select all option
if not exclusive and items:
self._ignore_next_toggle = False
self.select_all_checkbox = QCheckBox('all entries')
self.select_all_checkbox.setTristate(True)
self.select_all_checkbox.stateChanged.connect(
self._on_select_all_checkbox_stateChanged)
self.verticalLayout.addWidget(self.select_all_checkbox)
self.content.toggled.connect(self._on_main_toggle)
if self.checkitem1:
self.checkitem1_checkbox = QCheckBox(self.checkitem1)
self.checkitem1_checkbox.stateChanged.connect(
self._on_select_checkitem1_checkbox_stateChanged)
self.verticalLayout.addWidget(self.checkitem1_checkbox)
if self.checkitem2:
self.checkitem2_checkbox = QCheckBox(self.checkitem2)
self.checkitem2_checkbox.stateChanged.connect(
self._on_select_checkitem2_checkbox_stateChanged)
self.verticalLayout.addWidget(self.checkitem2_checkbox)
if not items:
spacerItem = QSpacerItem(1, 1, QSizePolicy.Expanding,
QSizePolicy.Expanding)
self.verticalLayout.addItem(spacerItem)
self._close_timer = None
self._closein = closein - 1
if closein > 0:
self.closein_label = QLabel("OK in %d sec..." % closein)
self.closein_label.setAlignment(Qt.AlignRight)
self.verticalLayout.addWidget(self.closein_label)
self._close_timer = threading.Timer(1.0, self._on_close_timer)
self._close_timer.start()
# create buttons
self.buttonBox = QDialogButtonBox(self)
self.buttonBox.setObjectName("buttonBox")
self.buttonBox.setOrientation(Qt.Horizontal)
self.buttonBox.setStandardButtons(buttons)
self.buttonBox.accepted.connect(self.accept)
self.buttonBox.rejected.connect(self.reject)
self.verticalLayout.addWidget(self.buttonBox)
# set the input fields
if items:
self.content.createFieldsFromValues(items, exclusive)
if (select_if_single and len(items) == 1) or preselect_all:
self.select_all_checkbox.setCheckState(Qt.Checked)
if not items or len(items) < 7:
self.filter_frame.setVisible(False)
# print '=============== create', self.objectName()
#
# def __del__(self):
# print "************ destroy", self.objectName()
def _on_main_toggle(self, state):
self.cancel_autoclose()
self._ignore_next_toggle = state != self.select_all_checkbox.checkState(
)
self.select_all_checkbox.setCheckState(state)
def _on_select_all_checkbox_stateChanged(self, state):
self.cancel_autoclose()
if not self._ignore_next_toggle:
self.content.setState(state)
self._ignore_next_toggle = False
def _on_select_checkitem1_checkbox_stateChanged(self, state):
self.cancel_autoclose()
if state == Qt.Checked:
self.checkitem1_result = True
elif state == Qt.Unchecked:
self.checkitem1_result = False
def _on_select_checkitem2_checkbox_stateChanged(self, state):
self.cancel_autoclose()
if state == Qt.Checked:
self.checkitem2_result = True
elif state == Qt.Unchecked:
self.checkitem2_result = False
def _on_filter_changed(self):
self.content.filter(self.filter_field.text())
def _on_close_timer(self):
self.closein_label.setText("OK in %d sec..." % self._closein)
if self._closein == 0:
self.buttonBox.accepted.emit()
return
self._closein -= 1
self._close_timer = threading.Timer(1.0, self._on_close_timer)
self._close_timer.start()
def cancel_autoclose(self):
if self._close_timer is not None:
self._close_timer.cancel()
self.closein_label.setVisible(False)
def getSelected(self):
return self.content.getSelected()
@staticmethod
def getValue(title,
description='',
items=list(),
exclusive=False,
preselect_all=False,
icon='',
parent=None,
select_if_single=True,
checkitem1='',
checkitem2='',
closein=0):
selectDia = SelectDialog(items,
exclusive=exclusive,
preselect_all=preselect_all,
description=description,
icon=icon,
parent=parent,
select_if_single=select_if_single,
checkitem1=checkitem1,
checkitem2=checkitem2,
closein=closein)
selectDia.setWindowTitle(title)
selectDia.resize(480, 256)
if selectDia.exec_():
if selectDia.checkitem2:
return selectDia.getSelected(
), True, selectDia.checkitem1_result, selectDia.checkitem2_result
if selectDia.checkitem1:
return selectDia.getSelected(
), True, selectDia.checkitem1_result
return selectDia.getSelected(), True
if selectDia.checkitem2:
return list(), False, False, False
if selectDia.checkitem1:
return list(), False, False
return list(), False
# %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
# %%%%%%%%%%%%%%%%%% close handling %%%%%%%%%%%%%%%%%%%%%
# %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
def accept(self):
self.cancel_autoclose()
self.setResult(QDialog.Accepted)
self.hide()
def reject(self):
self.cancel_autoclose()
self.setResult(QDialog.Rejected)
self.hide()
def hideEvent(self, event):
self.close()
def closeEvent(self, event):
'''
Test the open files for changes and save this if needed.
'''
self.cancel_autoclose()
self.setAttribute(Qt.WA_DeleteOnClose, True)
QDialog.closeEvent(self, event)
class SelectDialog(QDialog):
'''
This dialog creates an input mask for a string list and return selected entries.
'''
def __init__(self, items=list(), buttons=QDialogButtonBox.Cancel | QDialogButtonBox.Ok, exclusive=False,
preselect_all=False, title='', description='', icon='', parent=None, select_if_single=True,
checkitem1='', checkitem2=''):
'''
Creates an input dialog.
@param items: a list with strings
@type items: C{list()}
'''
QDialog.__init__(self, parent=parent)
self.setObjectName(' - '.join(['SelectDialog', str(items)]))
self.verticalLayout = QVBoxLayout(self)
self.verticalLayout.setObjectName("verticalLayout")
self.verticalLayout.setContentsMargins(1, 1, 1, 1)
# add filter row
self.filter_frame = QFrame(self)
filterLayout = QHBoxLayout(self.filter_frame)
filterLayout.setContentsMargins(1, 1, 1, 1)
label = QLabel("Filter:", self.filter_frame)
self.filter_field = EnchancedLineEdit(self.filter_frame)
filterLayout.addWidget(label)
filterLayout.addWidget(self.filter_field)
self.filter_field.textChanged.connect(self._on_filter_changed)
self.verticalLayout.addWidget(self.filter_frame)
if description:
self.description_frame = QFrame(self)
descriptionLayout = QHBoxLayout(self.description_frame)
# descriptionLayout.setContentsMargins(1, 1, 1, 1)
if icon:
self.icon_label = QLabel(self.description_frame)
self.icon_label.setSizePolicy(QSizePolicy.Fixed, QSizePolicy.Fixed)
self.icon_label.setPixmap(QPixmap(icon).scaled(30, 30, Qt.KeepAspectRatio))
descriptionLayout.addWidget(self.icon_label)
self.description_label = QLabel(self.description_frame)
self.description_label.setWordWrap(True)
self.description_label.setText(description)
descriptionLayout.addWidget(self.description_label)
self.verticalLayout.addWidget(self.description_frame)
# create area for the parameter
self.content = MainBox(self)
if items:
self.scroll_area = QScrollArea(self)
self.scroll_area.setFocusPolicy(Qt.NoFocus)
self.scroll_area.setObjectName("scroll_area")
self.scroll_area.setWidgetResizable(True)
self.scroll_area.setWidget(self.content)
self.verticalLayout.addWidget(self.scroll_area)
self.checkitem1 = checkitem1
self.checkitem1_result = False
self.checkitem2 = checkitem2
self.checkitem2_result = False
# add select all option
if not exclusive and items:
self._ignore_next_toggle = False
self.select_all_checkbox = QCheckBox('all entries')
self.select_all_checkbox.setTristate(True)
self.select_all_checkbox.stateChanged.connect(self._on_select_all_checkbox_stateChanged)
self.verticalLayout.addWidget(self.select_all_checkbox)
self.content.toggled.connect(self._on_main_toggle)
if self.checkitem1:
self.checkitem1_checkbox = QCheckBox(self.checkitem1)
self.checkitem1_checkbox.stateChanged.connect(self._on_select_checkitem1_checkbox_stateChanged)
self.verticalLayout.addWidget(self.checkitem1_checkbox)
if self.checkitem2:
self.checkitem2_checkbox = QCheckBox(self.checkitem2)
self.checkitem2_checkbox.stateChanged.connect(self._on_select_checkitem2_checkbox_stateChanged)
self.verticalLayout.addWidget(self.checkitem2_checkbox)
if not items:
spacerItem = QSpacerItem(1, 1, QSizePolicy.Expanding, QSizePolicy.Expanding)
self.verticalLayout.addItem(spacerItem)
# create buttons
self.buttonBox = QDialogButtonBox(self)
self.buttonBox.setObjectName("buttonBox")
self.buttonBox.setOrientation(Qt.Horizontal)
self.buttonBox.setStandardButtons(buttons)
self.buttonBox.accepted.connect(self.accept)
self.buttonBox.rejected.connect(self.reject)
self.verticalLayout.addWidget(self.buttonBox)
# set the input fields
if items:
self.content.createFieldsFromValues(items, exclusive)
if (select_if_single and len(items) == 1) or preselect_all:
self.select_all_checkbox.setCheckState(Qt.Checked)
if not items or len(items) < 7:
self.filter_frame.setVisible(False)
# print '=============== create', self.objectName()
#
# def __del__(self):
# print "************ destroy", self.objectName()
def _on_main_toggle(self, state):
self._ignore_next_toggle = state != self.select_all_checkbox.checkState()
self.select_all_checkbox.setCheckState(state)
def _on_select_all_checkbox_stateChanged(self, state):
if not self._ignore_next_toggle:
self.content.setState(state)
self._ignore_next_toggle = False
def _on_select_checkitem1_checkbox_stateChanged(self, state):
if state == Qt.Checked:
self.checkitem1_result = True
elif state == Qt.Unchecked:
self.checkitem1_result = False
def _on_select_checkitem2_checkbox_stateChanged(self, state):
if state == Qt.Checked:
self.checkitem2_result = True
elif state == Qt.Unchecked:
self.checkitem2_result = False
def _on_filter_changed(self):
self.content.filter(self.filter_field.text())
def getSelected(self):
return self.content.getSelected()
@staticmethod
def getValue(title, description='', items=list(), exclusive=False, preselect_all=False, icon='', parent=None, select_if_single=True, checkitem1='', checkitem2=''):
selectDia = SelectDialog(items, exclusive=exclusive, preselect_all=preselect_all, description=description, icon=icon, parent=parent, select_if_single=select_if_single, checkitem1=checkitem1, checkitem2=checkitem2)
selectDia.setWindowTitle(title)
selectDia.resize(480, 256)
if selectDia.exec_():
if selectDia.checkitem2:
return selectDia.getSelected(), True, selectDia.checkitem1_result, selectDia.checkitem2_result
if selectDia.checkitem1:
return selectDia.getSelected(), True, selectDia.checkitem1_result
return selectDia.getSelected(), True
if selectDia.checkitem2:
return list(), False, False, False
if selectDia.checkitem1:
return list(), False, False
return list(), False
# %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
# %%%%%%%%%%%%%%%%%% close handling %%%%%%%%%%%%%%%%%%%%%
# %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
def accept(self):
self.setResult(QDialog.Accepted)
self.hide()
def reject(self):
self.setResult(QDialog.Rejected)
self.hide()
def hideEvent(self, event):
self.close()
def closeEvent(self, event):
'''
Test the open files for changes and save this if needed.
'''
self.setAttribute(Qt.WA_DeleteOnClose, True)
QDialog.closeEvent(self, event)
class StringLabelWidget(QWidget):
def __init__(self):
super(StringLabelWidget, self).__init__()
self.lock = Lock()
vbox = QtGui.QVBoxLayout(self)
self.label = QLabel()
self.label.setAlignment(Qt.AlignLeft)
self.label.setSizePolicy(
QSizePolicy(QSizePolicy.Ignored, QSizePolicy.Ignored))
font = QFont("Helvetica", 14)
self.label.setFont(font)
self.label.setWordWrap(True)
vbox.addWidget(self.label)
self.string_sub = None
self._string_topics = []
self._update_topic_timer = QTimer(self)
self._update_topic_timer.timeout.connect(self.updateTopics)
self._update_topic_timer.start(1)
self._active_topic = None
self._dialog = ComboBoxDialog()
def trigger_configuration(self):
self._dialog.exec_()
self.setupSubscriber(self._string_topics[self._dialog.number])
def updateTopics(self):
need_to_update = False
for (topic, topic_type) in rospy.get_published_topics():
if topic_type == "std_msgs/String":
if not topic in self._string_topics:
self._string_topics.append(topic)
need_to_update = True
if need_to_update:
self._string_topics = sorted(self._string_topics)
self._dialog.combo_box.clear()
for topic in self._string_topics:
self._dialog.combo_box.addItem(topic)
if self._active_topic:
if self._active_topic not in self._string_topics:
self._string_topics.append(self._active_topic)
self._dialog.combo_box.addItem(self._active_topic)
self._dialog.combo_box.setCurrentIndex(
self._string_topics.index(self._active_topic))
def setupSubscriber(self, topic):
if self.string_sub:
self.string_sub.unregister()
self.string_sub = rospy.Subscriber(topic, String, self.stringCallback)
self._active_topic = topic
def onActivated(self, number):
self.setupSubscriber(self._string_topics[number])
def stringCallback(self, msg):
self.string = msg.data
self.label.setText(self.string)
def save_settings(self, plugin_settings, instance_settings):
if self._active_topic:
instance_settings.set_value("active_topic", self._active_topic)
def restore_settings(self, plugin_settings, instance_settings):
if instance_settings.value("active_topic"):
topic = instance_settings.value("active_topic")
self._dialog.combo_box.addItem(topic)
self.setupSubscriber(topic)
class EchoDialog(QDialog):
MESSAGE_LINE_LIMIT = 128
MESSAGE_HZ_LIMIT = 10
MAX_DISPLAY_MSGS = 25
STATISTIC_QUEUE_LEN = 5000
"""
This dialog shows the output of a topic.
"""
finished_signal = Signal(str)
"""
finished_signal has as parameter the name of the topic and is emitted, if this
dialog was closed.
"""
msg_signal = Signal(object, bool)
"""
msg_signal is a signal, which is emitted, if a new message was received.
"""
text_hz_signal = Signal(str)
text_signal = Signal(str)
"""
text_signal is a signal, which is emitted, if a new text to display was received.
"""
text_error_signal = Signal(str)
"""
text_error_signal is a signal, which is emitted, if a new error text to display was received.
"""
request_pw = Signal(object)
def __init__(self, topic, msg_type, show_only_rate=False, masteruri=None, use_ssh=False, parent=None):
"""
Creates an input dialog.
@param topic: the name of the topic
@type topic: C{str}
@param msg_type: the type of the topic
@type msg_type: C{str}
@raise Exception: if no topic class was found for the given type
"""
QDialog.__init__(self, parent=parent)
self._masteruri = masteruri
masteruri_str = "" if masteruri is None else "[%s]" % masteruri
self.setObjectName(" - ".join(["EchoDialog", topic, masteruri_str]))
self.setAttribute(Qt.WA_DeleteOnClose, True)
self.setWindowFlags(Qt.Window)
self.setWindowTitle("%s %s %s" % ("Echo --- " if not show_only_rate else "Hz --- ", topic, masteruri_str))
self.resize(728, 512)
self.verticalLayout = QVBoxLayout(self)
self.verticalLayout.setObjectName("verticalLayout")
self.verticalLayout.setContentsMargins(1, 1, 1, 1)
self.mIcon = QIcon(":/icons/crystal_clear_prop_run_echo.png")
self.setWindowIcon(self.mIcon)
self.topic = topic
self.show_only_rate = show_only_rate
self.lock = threading.RLock()
self.last_printed_count = 0
self.msg_t0 = -1.0
self.msg_tn = 0
self.times = []
self.message_count = 0
self._rate_message = ""
self._scrapped_msgs = 0
self._scrapped_msgs_sl = 0
self._last_received_ts = 0
self.receiving_hz = self.MESSAGE_HZ_LIMIT
self.line_limit = self.MESSAGE_LINE_LIMIT
self.field_filter_fn = None
options = QWidget(self)
if not show_only_rate:
hLayout = QHBoxLayout(options)
hLayout.setContentsMargins(1, 1, 1, 1)
self.no_str_checkbox = no_str_checkbox = QCheckBox("Hide strings")
no_str_checkbox.toggled.connect(self.on_no_str_checkbox_toggled)
hLayout.addWidget(no_str_checkbox)
self.no_arr_checkbox = no_arr_checkbox = QCheckBox("Hide arrays")
no_arr_checkbox.toggled.connect(self.on_no_arr_checkbox_toggled)
hLayout.addWidget(no_arr_checkbox)
self.combobox_reduce_ch = QComboBox(self)
self.combobox_reduce_ch.addItems([str(self.MESSAGE_LINE_LIMIT), "0", "80", "256", "1024"])
self.combobox_reduce_ch.activated[str].connect(self.combobox_reduce_ch_activated)
self.combobox_reduce_ch.setEditable(True)
self.combobox_reduce_ch.setToolTip("Set maximum line width. 0 disables the limit.")
hLayout.addWidget(self.combobox_reduce_ch)
# reduce_ch_label = QLabel('ch', self)
# hLayout.addWidget(reduce_ch_label)
# add spacer
spacerItem = QSpacerItem(515, 20, QSizePolicy.Expanding, QSizePolicy.Minimum)
hLayout.addItem(spacerItem)
# add combobox for displaying frequency of messages
self.combobox_displ_hz = QComboBox(self)
self.combobox_displ_hz.addItems([str(self.MESSAGE_HZ_LIMIT), "0", "0.1", "1", "50", "100", "1000"])
self.combobox_displ_hz.activated[str].connect(self.on_combobox_hz_activated)
self.combobox_displ_hz.setEditable(True)
hLayout.addWidget(self.combobox_displ_hz)
displ_hz_label = QLabel("Hz", self)
hLayout.addWidget(displ_hz_label)
# add combobox for count of displayed messages
self.combobox_msgs_count = QComboBox(self)
self.combobox_msgs_count.addItems([str(self.MAX_DISPLAY_MSGS), "0", "50", "100"])
self.combobox_msgs_count.activated[str].connect(self.on_combobox_count_activated)
self.combobox_msgs_count.setEditable(True)
self.combobox_msgs_count.setToolTip("Set maximum displayed message count. 0 disables the limit.")
hLayout.addWidget(self.combobox_msgs_count)
displ_count_label = QLabel("#", self)
hLayout.addWidget(displ_count_label)
# add topic control button for unsubscribe and subscribe
self.topic_control_button = QToolButton(self)
self.topic_control_button.setText("stop")
self.topic_control_button.setIcon(QIcon(":/icons/deleket_deviantart_stop.png"))
self.topic_control_button.clicked.connect(self.on_topic_control_btn_clicked)
hLayout.addWidget(self.topic_control_button)
# add clear button
clearButton = QToolButton(self)
clearButton.setText("clear")
clearButton.clicked.connect(self.on_clear_btn_clicked)
hLayout.addWidget(clearButton)
self.verticalLayout.addWidget(options)
self.display = QTextBrowser(self)
self.display.setReadOnly(True)
self.verticalLayout.addWidget(self.display)
self.display.document().setMaximumBlockCount(500)
self.max_displayed_msgs = self.MAX_DISPLAY_MSGS
self._blocks_in_msg = None
self.display.setOpenLinks(False)
self.display.anchorClicked.connect(self._on_display_anchorClicked)
self.status_label = QLabel("0 messages", self)
self.verticalLayout.addWidget(self.status_label)
# subscribe to the topic
errmsg = ""
try:
self.__msg_class = message.get_message_class(msg_type)
if not self.__msg_class:
errmsg = "Cannot load message class for [%s]. Did you build messages?" % msg_type
# raise Exception("Cannot load message class for [%s]. Did you build messages?"%msg_type)
except Exception as e:
self.__msg_class = None
errmsg = "Cannot load message class for [%s]. Did you build messagest?\nError: %s" % (msg_type, e)
# raise Exception("Cannot load message class for [%s]. Did you build messagest?\nError: %s"%(msg_type, e))
# variables for Subscriber
self.msg_signal.connect(self._append_message)
self.sub = None
# vairables for SSH connection
self.ssh_output_file = None
self.ssh_error_file = None
self.ssh_input_file = None
self.text_signal.connect(self._append_text)
self.text_hz_signal.connect(self._append_text_hz)
self._current_msg = ""
self._current_errmsg = ""
self.text_error_signal.connect(self._append_error_text)
# decide, which connection to open
if use_ssh:
self.__msg_class = None
self._on_display_anchorClicked(QUrl(self._masteruri))
elif self.__msg_class is None:
errtxt = '<pre style="color:red; font-family:Fixedsys,Courier,monospace; padding:10px;">\n%s</pre>' % (
errmsg
)
self.display.setText('<a href="%s">open using SSH</a>' % (masteruri))
self.display.append(errtxt)
else:
self.sub = rospy.Subscriber(self.topic, self.__msg_class, self._msg_handle)
self.print_hz_timer = QTimer()
self.print_hz_timer.timeout.connect(self._on_calc_hz)
self.print_hz_timer.start(1000)
# print "======== create", self.objectName()
#
# def __del__(self):
# print "******* destroy", self.objectName()
# def hideEvent(self, event):
# self.close()
def closeEvent(self, event):
if self.sub is not None:
self.sub.unregister()
del self.sub
try:
self.ssh_output_file.close()
self.ssh_error_file.close()
# send Ctrl+C to remote process
self.ssh_input_file.write("%s\n" % chr(3))
self.ssh_input_file.close()
except:
pass
self.finished_signal.emit(self.topic)
if self.parent() is None:
QApplication.quit()
# else:
# self.setParent(None)
def create_field_filter(self, echo_nostr, echo_noarr):
def field_filter(val):
try:
# fields = val.__slots__
# field_types = val._slot_types
for f, t in zip(val.__slots__, val._slot_types):
if echo_noarr and "[" in t:
continue
elif echo_nostr and "string" in t:
continue
yield f
except:
pass
return field_filter
def on_no_str_checkbox_toggled(self, state):
self.field_filter_fn = self.create_field_filter(state, self.no_arr_checkbox.isChecked())
def on_no_arr_checkbox_toggled(self, state):
self.field_filter_fn = self.create_field_filter(self.no_str_checkbox.isChecked(), state)
def combobox_reduce_ch_activated(self, ch_txt):
try:
self.line_limit = int(ch_txt)
except ValueError:
try:
self.line_limit = float(ch_txt)
except ValueError:
self.combobox_reduce_ch.setEditText(str(self.line_limit))
def on_combobox_hz_activated(self, hz_txt):
try:
self.receiving_hz = int(hz_txt)
except ValueError:
try:
self.receiving_hz = float(hz_txt)
except ValueError:
self.combobox_displ_hz.setEditText(str(self.receiving_hz))
def on_combobox_count_activated(self, count_txt):
try:
self.max_displayed_msgs = int(count_txt)
self._blocks_in_msg = None
except ValueError:
self.combobox_msgs_count.setEditText(str(self.max_displayed_msgs))
def on_clear_btn_clicked(self):
self.display.clear()
with self.lock:
self.message_count = 0
self._scrapped_msgs = 0
del self.times[:]
def on_topic_control_btn_clicked(self):
try:
if self.sub is None and self.ssh_output_file is None:
if self.__msg_class:
self.sub = rospy.Subscriber(self.topic, self.__msg_class, self._msg_handle)
else:
self._on_display_anchorClicked(QUrl(self._masteruri))
self.topic_control_button.setText("stop")
self.topic_control_button.setIcon(QIcon(":/icons/deleket_deviantart_stop.png"))
else:
if self.sub is not None:
self.sub.unregister()
self.sub = None
elif self.ssh_output_file is not None:
self.ssh_output_file.close()
self.ssh_error_file.close()
self.ssh_output_file = None
self.topic_control_button.setText("play")
self.topic_control_button.setIcon(QIcon(":/icons/deleket_deviantart_play.png"))
self.no_str_checkbox.setEnabled(True)
self.no_arr_checkbox.setEnabled(True)
except Exception as e:
rospy.logwarn("Error while stop/play echo for topic %s: %s" % (self.topic, e))
def _msg_handle(self, data):
self.msg_signal.emit(data, (data._connection_header["latching"] != "0"))
def _append_message(self, msg, latched):
"""
Adds a label to the dialog's layout and shows the given text.
@param msg: the text to add to the dialog
@type msg: message object
"""
current_time = time.time()
self._count_messages(current_time)
# skip messages, if they are received often then MESSAGE_HZ_LIMIT
if self._last_received_ts != 0 and self.receiving_hz != 0:
if not latched and current_time - self._last_received_ts < 1.0 / self.receiving_hz:
self._scrapped_msgs += 1
self._scrapped_msgs_sl += 1
return
self._last_received_ts = current_time
if not self.show_only_rate:
# convert message to string and reduce line width to current limit
msg = message.strify_message(msg, field_filter=self.field_filter_fn)
if isinstance(msg, tuple):
msg = msg[0]
msg = self._trim_width(msg)
msg = msg.replace("<", "<").replace(">", ">")
# create a notification about scrapped messages
if self._scrapped_msgs_sl > 0:
txt = (
'<pre style="color:red; font-family:Fixedsys,Courier,monospace; padding:10px;">scrapped %s message because of Hz-settings</pre>'
% self._scrapped_msgs_sl
)
self.display.append(txt)
self._scrapped_msgs_sl = 0
txt = (
'<pre style="background-color:#FFFCCC; font-family:Fixedsys,Courier; padding:10px;">---------- %s --------------------\n%s</pre>'
% (datetime.now().strftime("%d.%m.%Y %H:%M:%S.%f"), msg)
)
# set the count of the displayed messages on receiving the first message
self._update_max_msg_count(txt)
self.display.append(txt)
self._print_status()
def _count_messages(self, ts=time.time()):
"""
Counts the received messages. Call this method only on receive message.
"""
current_time = ts
with self.lock:
# time reset
if self.msg_t0 < 0 or self.msg_t0 > current_time:
self.msg_t0 = current_time
self.msg_tn = current_time
self.times = []
else:
self.times.append(current_time - self.msg_tn)
self.msg_tn = current_time
# keep only statistics for the last 5000 messages so as not to run out of memory
if len(self.times) > self.STATISTIC_QUEUE_LEN:
self.times.pop(0)
self.message_count += 1
def _trim_width(self, msg):
"""
reduce line width to current limit
:param msg: the message
:type msg: str
:return: trimmed message
"""
result = msg
if self.line_limit != 0:
a = ""
for l in msg.splitlines():
a = a + (l if len(l) <= self.line_limit else l[0 : self.line_limit - 3] + "...") + "\n"
result = a
return result
def _update_max_msg_count(self, txt):
"""
set the count of the displayed messages on receiving the first message
:param txt: text of the message, which will be added to the document
:type txt: str
"""
if self._blocks_in_msg is None:
td = QTextDocument(txt)
self._blocks_in_msg = td.blockCount()
self.display.document().setMaximumBlockCount(self._blocks_in_msg * self.max_displayed_msgs)
def _on_calc_hz(self):
if rospy.is_shutdown():
self.close()
return
if self.message_count == self.last_printed_count:
return
with self.lock:
# the code from ROS rostopic
n = len(self.times)
if n < 2:
return
mean = sum(self.times) / n
rate = 1.0 / mean if mean > 0.0 else 0
# std dev
std_dev = math.sqrt(sum((x - mean) ** 2 for x in self.times) / n)
# min and max
max_delta = max(self.times)
min_delta = min(self.times)
self.last_printed_count = self.message_count
self._rate_message = "average rate: %.3f\tmin: %.3fs max: %.3fs std dev: %.5fs window: %s" % (
rate,
min_delta,
max_delta,
std_dev,
n + 1,
)
if self._scrapped_msgs > 0:
self._rate_message += " --- scrapped msgs: %s" % self._scrapped_msgs
self._print_status()
if self.show_only_rate:
self.display.append(self._rate_message)
def _print_status(self):
self.status_label.setText("%s messages %s" % (self.message_count, self._rate_message))
def _append_text(self, text):
"""
Append echo text received through the SSH.
"""
with self.lock:
self._current_msg += text
if self._current_msg.find("---") != -1:
messages = self._current_msg.split("---")
for m in messages[:-1]:
current_time = time.time()
self._count_messages(current_time)
# limit the displayed text width
m = self._trim_width(m)
txt = (
'<pre style="background-color:#FFFCCC; font-family:Fixedsys,Courier; padding:10px;">---------- %s --------------------\n%s</pre>'
% (datetime.now().strftime("%d.%m.%Y %H:%M:%S.%f"), m)
)
# set the count of the displayed messages on receiving the first message
self._update_max_msg_count(txt)
self.display.append(txt)
self._current_msg = messages[-1]
self._print_status()
def _append_error_text(self, text):
"""
Append error text received through the SSH.
"""
with self.lock:
self._current_errmsg += text
if self._current_errmsg.find("\n") != -1:
messages = self._current_errmsg.split("\n")
for m in messages[:-1]:
txt = '<pre style="color:red; font-family:Fixedsys,Courier,monospace; padding:10px;">%s</pre>' % m
self.display.append(txt)
self._current_errmsg = messages[-1]
def _append_text_hz(self, text):
"""
Append text received through the SSH for hz view.
"""
with self.lock:
self._current_msg += text
if self._current_msg.find("\n") != -1:
messages = self._current_msg.split("\n")
for m in messages[:-1]:
txt = '<div style="font-family:Fixedsys,Courier;">%s</div>' % (m)
self.display.append(txt)
self._current_msg = messages[-1]
def _on_display_anchorClicked(self, url, user=None, pw=None):
try:
ok = False
if self.show_only_rate:
self.ssh_input_file, self.ssh_output_file, self.ssh_error_file, ok = nm.ssh().ssh_exec(
url.host(), ["rostopic hz %s" % (self.topic)], user, pw, auto_pw_request=True, get_pty=True
)
self.status_label.setText("connected to %s over SSH" % url.host())
else:
self.combobox_displ_hz.setEnabled(False)
nostr = "--nostr" if self.no_str_checkbox.isChecked() else ""
noarr = "--noarr" if self.no_arr_checkbox.isChecked() else ""
self.ssh_input_file, self.ssh_output_file, self.ssh_error_file, ok = nm.ssh().ssh_exec(
url.host(),
["rostopic echo %s %s %s" % (nostr, noarr, self.topic)],
user,
pw,
auto_pw_request=True,
get_pty=True,
)
if ok:
self.display.clear()
target = self._read_output_hz if self.show_only_rate else self._read_output
thread = threading.Thread(target=target, args=((self.ssh_output_file,)))
thread.setDaemon(True)
thread.start()
thread = threading.Thread(target=self._read_error, args=((self.ssh_error_file,)))
thread.setDaemon(True)
thread.start()
elif self.ssh_output_file:
self.ssh_output_file.close()
self.ssh_error_file.close()
except Exception as e:
self._append_error_text("%s\n" % e)
# import traceback
# print traceback.format_exc()
def _read_output_hz(self, output_file):
try:
while not output_file.closed:
text = output_file.read(1)
if text:
self.text_hz_signal.emit(text)
except:
pass
# import traceback
# print traceback.format_exc()
def _read_output(self, output_file):
while not output_file.closed:
text = output_file.read(1)
if text:
self.text_signal.emit(text)
def _read_error(self, error_file):
try:
while not error_file.closed:
text = error_file.read(1)
if text:
self.text_error_signal.emit(text)
except:
pass
