class RosMsgView(QGroupBox):
def __init__(self,parent,name,topic_name,topic_type,topic_fields):
super(RosMsgView,self).__init__(name,parent)
self.topic_name = topic_name
self.topic_type = topic_type
self.topic_fields = topic_fields
self.ref_dict = {}
self.layout = QGridLayout()
self.__populate_self()
self.subscriber = RosHelper.create_subscriber_from_type(self.topic_name,self.topic_type,self.onmsg)
def __populate_self(self):
i = 1
if len(self.topic_fields) == 0:
fields = RosHelper.get_all_msg_fields(self.topic_type)
else:
fields = self.topic_fields
for field in fields:
self.ref_dict[field] = QLabel("None",self)
self.layout.addWidget(QLabel(field,self),i,1)
self.layout.addWidget(self.ref_dict[field],i,2)
i+=1
self.setLayout(self.layout)
def onmsg(self,msg):
# loop msg with topic field and
for topic_field in self.topic_fields:
r = msg
lbl = self.ref_dict[topic_field]
for subfields in topic_field.split(".")[1:]:
r = getattr(r,subfields)
lbl.setText(str(r))
def __init__(self):
QWidget.__init__(self)
self._status = "unknown"
self._layout = QGridLayout(self)
self._widgets = dict()
self.__logging_cb_signal.connect(self.logging_cb)
self.__start_logging_signal.connect(self.start_logging)
self.__stop_logging_signal.connect(self.stop_logging)
self._add_widget("pid_label", QLabel("PID:", self))
self._add_widget("pid", QLineEdit(self))
self._widgets["pid"].setMaximumWidth(50)
self._add_widget("label_status", QLabel(self))
self._widgets["label_status"].setText("Status: Unknown")
self._widgets["label_status"].setTextFormat(QtCore.Qt.RichText)
self._add_widget("button_start", QPushButton("START", self))
self._add_widget("button_stop", QPushButton("STOP", self))
self._widgets["button_start"].clicked.connect(
self.__start_logging_signal.emit)
self._widgets["button_stop"].clicked.connect(
self.__stop_logging_signal.emit)
self._widgets["label_warning"] = QLabel(self)
self._layout.addWidget(self._widgets["label_warning"], 1, 0, 1, 2)
self._widgets["label_warning"].setTextFormat(QtCore.Qt.RichText)
rospy.Subscriber("/data_logger/status", String,
self.__logging_cb_signal.emit)
self._logging_start = rospy.ServiceProxy('/data_logger/start',
DataLoggerStart)
self._logging_stop = rospy.ServiceProxy('/data_logger/stop',
DataLoggerStop)
def __init__(self):
super(TemporaryTask_dialog, self).__init__()
self.setObjectName('TemporaryTask_dialog')
# Load ui file
ui_file = os.path.join(rospkg.RosPack().get_path('rqt_simulation'),
'resource', 'temporary_task.ui')
loadUi(ui_file, self)
self.button_add.clicked.connect(self.add_eventually)
self.button_remove.clicked.connect(self.remove_eventually)
self.button_cancel.clicked.connect(self.cancel)
self.button_send.clicked.connect(self.send_task)
# Make whole dialog scrollable
self.grid = QGridLayout()
self.scrollAreaWidgetContents.setLayout(self.grid)
self.atomic_propositions = []
self.eventually_label_list = []
eventually_label = QLabel('<> (')
self.eventually_label_list.append(eventually_label)
self.grid.addWidget(eventually_label, 0, 0)
self.eventually_input_list = []
eventually_input = QLineEdit()
self.eventually_input_list.append(eventually_input)
self.grid.addWidget(eventually_input, 0, 1)
self.end_bracket_list = []
end_bracket = QLabel(')')
self.end_bracket_list.append(end_bracket)
self.grid.addWidget(end_bracket, 0, 2)
def __init__(self,parent,name,topic_name,topic_type,topic_fields):
super(RosMsgView,self).__init__(name,parent)
self.topic_name = topic_name
self.topic_type = topic_type
self.topic_fields = topic_fields
self.ref_dict = {}
self.layout = QGridLayout()
self.__populate_self()
self.subscriber = RosHelper.create_subscriber_from_type(self.topic_name,self.topic_type,self.onmsg)
def __init__(self):
super(Dashboard, self).__init__()
not_latched = False
# latched = True
self.publishers = py_trees_ros.utilities.Publishers([
('scan', "~scan", std_msgs.Empty, not_latched, 1),
('cancel', "~cancel", std_msgs.Empty, not_latched, 1),
])
self.scan_push_button = QPushButton("Scan")
self.scan_push_button.setStyleSheet("QPushButton { font-size: 30pt; }")
self.scan_push_button.setSizePolicy(QSizePolicy.Expanding,
QSizePolicy.Expanding)
self.scan_push_button.pressed.connect(
functools.partial(self.publish_button_message,
self.publishers.scan))
self.cancel_push_button = QPushButton("Cancel")
self.cancel_push_button.setStyleSheet(
"QPushButton { font-size: 30pt; }")
self.cancel_push_button.setSizePolicy(QSizePolicy.Expanding,
QSizePolicy.Expanding)
self.cancel_push_button.pressed.connect(
functools.partial(self.publish_button_message,
self.publishers.cancel))
self.led_strip_flashing = False
self.led_strip_on_count = 1
self.led_strip_colour = "grey"
self.led_strip_lock = threading.Lock()
self.led_strip_timer = QTimer()
self.led_strip_timer.timeout.connect(self.led_strip_timer_callback)
self.led_strip_label = QLabel("LED Strip")
self.led_strip_label.setAlignment(Qt.AlignCenter)
self.led_strip_label.setSizePolicy(QSizePolicy.Expanding,
QSizePolicy.Expanding)
self.led_strip_label.setStyleSheet(
"background-color: %s; font-size: 30pt;" % self.led_strip_colour)
self.hbox_layout = QGridLayout(self)
self.hbox_layout.addWidget(self.scan_push_button)
self.hbox_layout.addWidget(self.cancel_push_button)
self.hbox_layout.addWidget(self.led_strip_label)
self.subscribers = py_trees_ros.utilities.Subscribers([
("report", "/tree/report", std_msgs.String,
self.reality_report_callback),
("led_strip", "/led_strip/display", std_msgs.String,
self.led_strip_display_callback)
])
self.led_strip_timer.start(500) # ms
def __init__(self, context, actions):
super(ActionClientContainerWidget, self).__init__()
grid = QGridLayout()
grid.setSpacing(1)
MAX_COLUMNS = 2
self.setObjectName('ActionClientContainerWidget')
self.clear_button = QPushButton("Clear all")
self.clear_button.clicked[bool].connect(self._handle_clear_clicked)
grid.addWidget(self.clear_button, 0, 0)
self.widgets = []
row = 1
column = 0
for k in sorted(actions.keys()):
action_name = k
widget = ActionClientWidget(context, action_name, actions[k])
grid.addWidget(widget, row, column)
self.widgets.append(widget)
column += 1
if column >= MAX_COLUMNS:
row += 1
column = 0
self.setLayout(grid)
def create_layout(layout_list):
"""Create a button layout with a given list of buttons.
:param layout_list:
A list which contains multiple list which represent a row with given items
:return:
A populated QGridLayout object which contains the passed input buttons
"""
qt_button_layout = QGridLayout()
for row in range(len(layout_list)):
for column in range(len(layout_list[row])):
user_input_object = layout_list[row][column]
qt_button_layout.addWidget(user_input_object, row, column, 1, 1)
return qt_button_layout
def beginGroup(self,obj):
parent,layout = self.__get_immediate_parent()
panel = QGroupBox(obj.name,parent)
if obj.layout == "grid":
l = QGridLayout()
elif obj.layout == "vertical":
l = QVBoxLayout()
else:
l = QHBoxLayout()
self.__increase_nesting_level(panel, l)
def __init__(self, bagFiles, parent=None):
super(DiffTab, self).__init__()
self.parent = parent
self.layout = QGridLayout()
self.bagFiles = bagFiles
self.selectedValue = ('', '') # contains value as a tupel like ('<message>', '<value>')
# init the widgets
self.thresholdSetter = ThresholdSetter(self)
self.valueWidget = ValueSelectorWidget(self, False)
self.idSelector = IDSelectorWidget()
self.idSelector.setTitle("Select GT-ObjectID")
# connect the signals to the slots)
self.valueWidget.valueTreeWidget.itemClicked.connect(self.valueSelected)
#layout
self.layout.addWidget(self.thresholdSetter, 0, 0)
self.layout.addWidget(self.valueWidget, 0, 1)
self.layout.addWidget(self.idSelector, 0, 2)
self.setLayout(self.layout)
def __init__(self, title="block_pick_and_place"):
super(block_pick_and_place, self).__init__()
# Initial
self.br = tf.TransformBroadcaster()
self.tf_listener = tf.TransformListener()
self.ik = [0, 0, 0, 0]
# Thread lock
moveit_commander.roscpp_initialize(sys.argv)
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self.group = moveit_commander.MoveGroupCommander("arm")
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=1)
self.gripper_cmd = rospy.Publisher('/gripper_joint/command',
Float64,
queue_size=1)
# safe shutdown
rospy.on_shutdown(self.onShutdown)
# timer
rospy.loginfo("[%s] Initialized " % (rospy.get_name()))
self.group.allow_replanning(True)
self.group.set_pose_reference_frame("base_link")
self.group.set_goal_position_tolerance(0.005)
self.group.set_goal_orientation_tolerance(0.05)
self.home_pose()
self.init_interactive_marker()
self.arm_plan_state = False
self.vlayout = QVBoxLayout(self)
self.gridlayout = QGridLayout()
self.vlayout.addLayout(self.gridlayout)
# Buttons for randomizing and centering sliders and
# Spinbox for on-the-fly selecting number of rows
self.CWbutton = QPushButton('Start_to_plan', self)
self.CWbutton.clicked.connect(self.start_event)
self.vlayout.addWidget(self.CWbutton)
self.SPbutton = QPushButton('Stop', self)
self.SPbutton.clicked.connect(self.stop_event)
self.vlayout.addWidget(self.SPbutton)
def __init__(self, context):
super(QuestionDialogPlugin, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('QuestionDialogPlugin')
font_size = rospy.get_param("~font_size", 40)
# Create QWidget
self._widget = QWidget()
self._widget.setFont(QFont("Times", font_size, QFont.Bold))
self._layout = QVBoxLayout(self._widget)
self._text_browser = QTextBrowser(self._widget)
self._layout.addWidget(self._text_browser)
self._button_layout = QGridLayout()
self._layout.addLayout(self._button_layout)
# layout = QVBoxLayout(self._widget)
# layout.addWidget(self.button)
self._widget.setObjectName('QuestionDialogPluginUI')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
context.add_widget(self._widget)
# Setup service provider
self.service = rospy.Service('question_dialog', QuestionDialog,
self.service_callback)
self.request = None
self.response_ready = False
self.response = None
self.buttons = []
self.text_label = None
self.text_input = None
self.update_sig.connect(self.update)
self.timeout_sig.connect(self.timeout)
def __init__(self, bagFiles, parent=None):
super(RawDataTab, self).__init__()
self.parent = parent
self.layout = QGridLayout()
self.bagFiles = bagFiles
self.selectedSource = 2 # no bag is selected
self.selectedValue = (
'', '') # contains value as a tupel like ('<message>', '<value>')
# init the widgets
self.sourceSelector = RawDataSelector(self)
self.layout.addWidget(self.sourceSelector, 0, 0)
self.valueWidget = ValueSelectorWidget()
self.layout.addWidget(self.valueWidget, 0, 1)
self.idSelector = IDSelectorWidget()
self.layout.addWidget(self.idSelector, 0, 2)
# connect the signals to the slots
self.sourceSelector.dataSourceChanged.connect(self.sourceChanged)
self.valueWidget.valueTreeWidget.itemClicked.connect(
self.valueSelected)
self.setLayout(self.layout)
def create_subgait(self, name, subgait, version_selection):
subgait_group_box = QGroupBox()
subgait_group_box.setLayout(QGridLayout())
subgait_group_box.setObjectName('Subgait')
subgait_group_box.setTitle(name)
try:
version_name = version_selection[name]
except TypeError:
version_name = None
except KeyError:
version_name = None
dropdown = self.create_dropdown(subgait, version_name)
subgait_group_box.layout().addWidget(dropdown, 0, 0)
return subgait_group_box
def beginRosLabel(self,obj):
pm,lm = self.__get_immediate_parent()
fr = QFrame(pm)
layout = QGridLayout()
nlb = QLabel(obj.label_name + ":",fr)
nlb.setToolTip(obj.topic_name)
layout.addWidget(nlb,1,1)
layout.addWidget(RosLabel(fr,obj.label_name,obj.topic_name,obj.topic_type,obj.topic_field),1,2)
fr.setLayout(layout)
lm.addWidget(fr)
def __init__(self, title, jsp, num_rows=0):
super(JointStatePublisherGui, self).__init__()
font = QFont("Helvetica", 9, QFont.Bold)
self.hlayout = QHBoxLayout(self)
vlayout = QVBoxLayout()
glayout = QGridLayout()
right_l_lauout = QVBoxLayout()
self.listVeiw = QListWidget()
self.checkbox = []
self.value_line_edit = []
self.sliders = []
self.positions = []
self.progressbars = []
self.value_last = []
speed_max = enviromnt_conf['joint_speed']
slider_max = speed_max * 1000
position_max = enviromnt_conf['joint_max_position']
progress_max = position_max * 1000
#create joints widget
for i in range(0, num_rows):
if config[i][0]:
g_in_g = QGridLayout()
checkbox = QCheckBox(config[i][1])
checkbox.setFont(font)
self.checkbox.append(checkbox)
value_line_edit = QLineEdit()
value_line_edit.setFont(font)
value_line_edit.setText("0.0")
self.value_line_edit.append(value_line_edit)
display_lable = QLabel()
display_lable.setFont(font)
display_lable.setText("Position:")
position_label = QLabel()
position_label.setFont(font)
position_label.setText("0.0")
self.positions.append(position_label)
position_progress_bar = QProgressBar()
position_progress_bar.setMaximum(progress_max)
position_progress_bar.setMinimum(-progress_max)
position_progress_bar.setValue(0)
self.progressbars.append(position_progress_bar)
slider = QSlider()
slider.setMaximum(slider_max)
slider.setMinimum(-slider_max)
slider.setOrientation(Qt.Horizontal)
slider.valueChanged.connect(self.slider_value_changed)
self.sliders.append(slider)
g_in_g.addWidget(checkbox, 0, 0)
g_in_g.addWidget(value_line_edit, 0, 1)
g_in_g.addWidget(display_lable, 0, 2)
g_in_g.addWidget(position_label, 0, 3)
g_in_g.addWidget(slider, 1, 0, 1, 2)
g_in_g.addWidget(position_progress_bar, 1, 2, 1, 2)
glayout.addLayout(g_in_g, i, 0)
#create v layout
self.import_Btn = QPushButton('Import')
self.import_Btn.setFont(font)
self.import_Btn.clicked.connect(self.import_Btn_clecked)
self.export_Btn = QPushButton('Export')
self.export_Btn.setFont(font)
self.export_Btn.clicked.connect(self.export_Btn_clicked)
self.start_Btn = QPushButton("Start")
self.start_Btn.setFont(font)
self.start_Btn.clicked.connect(self.start_Btn_clicked)
self.reset_Btn = QPushButton('Reset')
self.reset_Btn.setFont(font)
self.reset_Btn.clicked.connect(self.reset_Btn_clicked)
self.record_Btn = QPushButton('Record')
self.record_Btn.setFont(font)
self.record_Btn.clicked.connect(self.record_Btn_clicked)
self.replay_Btn = QPushButton('Repaly')
self.replay_Btn.setFont(font)
self.replay_Btn.clicked.connect(self.replay_Btn_clicked)
self.delete_Btn = QPushButton("Delete")
self.delete_Btn.setFont(font)
self.delete_Btn.clicked.connect(self.delete_Btn_clicked)
self.debug_Btn = QPushButton("Debug")
self.debug_Btn.setFont(font)
self.debug_Btn.clicked.connect(self.debug_Btn_clicked)
vlayout.addWidget(self.import_Btn)
vlayout.addWidget(self.export_Btn)
vlayout.addWidget(self.start_Btn)
vlayout.addWidget(self.reset_Btn)
vlayout.addWidget(self.record_Btn)
vlayout.addWidget(self.delete_Btn)
vlayout.addWidget(self.replay_Btn)
vlayout.addWidget(self.debug_Btn)
self.master_url = QLineEdit("http://192.168.0.91:11311")
self.master_url.setFont(font)
self.master_ip = QLineEdit("192.168.0.91")
self.master_ip.setFont(font)
self.listVeiw.clicked.connect(self.listVeiw_clicked)
self.listVeiw.currentRowChanged.connect(
self.listVeiw_itemSelectionChanged)
self.description = QTextEdit("")
self.description.setFont(font)
#self.description.setGeometry(0,100,100,500)
right_l_lauout.addWidget(self.master_url)
right_l_lauout.addWidget(self.master_ip)
right_l_lauout.addWidget(self.listVeiw)
right_l_lauout.addWidget(self.description)
right_l_lauout.setStretch(0, 1)
right_l_lauout.setStretch(1, 1)
right_l_lauout.setStretch(2, 3)
right_l_lauout.setStretch(3, 1)
self.num_rows = len(self.checkbox)
self.hlayout.addLayout(glayout)
self.hlayout.addLayout(vlayout)
self.hlayout.addLayout(right_l_lauout)
self.setLayout(self.hlayout)
self.callback_start = None
self.callback_pause = None
self.callback_record = None
self.callback_reset = None
self.callback_replay = None
self.callback_replay_stop = None
self.callback_delete = None
self.callback_debug = None
self.callback_import = None
self.callback_export = None
self.callback_list_clicked = None
self.listVeiw_isClicked = False
self.listVeiw_current_item = 0
self.listVeiw_len = 0
self.f = QFileDialog()
class EventTransmissionGUI(Plugin):
"""
GUI to send events from User to logic state machine
"""
def __init__(self, context):
"""
Create Qt GUI using the event file
"""
super(EventTransmissionGUI, self).__init__(context)
self.setObjectName('ManualEventTriggerGUI')
parser = ArgumentParser()
# Add argument(s) to the parser.
args = self._parse_args(context.argv())
## Create Event trigger
self.event_trigger = RosEventTrigger(args.event_file)
## Parent container to store buttons, textboxes
self._container = QTabWidget()
## Tab to display robot system, state machine status
## and command buttons without any arguments
self._status_tab = QWidget()
## Tab to display positionyaw and velocityyaw commands
## and sliders for them
self._additional_commands_tab = QWidget()
# Set title of the parent container window
self._status_tab.setWindowTitle(self.event_trigger.event_manager_name)
## Layout for status tab
self._layout = QVBoxLayout()
self._status_tab.setLayout(self._layout)
# Create Textboxes and add to Layout
self._layout.addWidget(QLabel('State Machine State'))
## Textbox to show sytem status
self.system_status_textbox = QTextEdit()
self.system_status_textbox.setReadOnly(True)
self._layout.addWidget(self.system_status_textbox)
# Define and connect buttons
self._layout.addWidget(QLabel('Event Triggers'))
## Continer to store event triggering buttons
self.button_container = QWidget()
## List of push buttons to trigger events
self.push_buttons = list()
## Layout for the push buttons
self.button_layout = QGridLayout()
self.button_container.setLayout(self.button_layout)
button_index = 0
for event_name in self.event_trigger.event_names_list:
self.push_buttons.append(QPushButton(event_name))
partial_fun = partial(self.event_trigger.triggerEvent,
event_name=event_name)
self.push_buttons[-1].clicked.connect(partial_fun)
row, col = self.get_row_col(button_index, args.grid_cols)
self.button_layout.addWidget(self.push_buttons[-1], row, col)
button_index += 1
self._layout.addWidget(self.button_container)
## Layout for additional commands tab
self._additional_commands_layout = QVBoxLayout()
self._additional_commands_tab.setLayout(
self._additional_commands_layout)
# Create height slider
self._additional_commands_layout.addWidget(
QLabel('Pose Command Height (m)'))
## Height slider to adjust z coordinate for pose command
## \todo Matt: Load slider settings from param file
self.height_slider = self.createSlider(1.0, 20.0, 2.0, 1.0)
self._additional_commands_layout.addWidget(self.height_slider)
## Add button for triggering pose command
## Container for pose event related objects: slide etc
## \todo Matt: Reset slider value based on current quad height
self.pose_command_container = QWidget()
## Pose command layout
self.pose_command_layout = QGridLayout()
self.pose_command_container.setLayout(self.pose_command_layout)
## x pose label to display position command from rviz to user
self.pose_x = QLabel('x: -')
## y pose label to display position command from rviz to user
self.pose_y = QLabel('y: -')
## z pose label to display position command from rviz to user
self.pose_z = QLabel("z: {0:.2f}".format(self.height_slider.value()))
self.height_slider.valueChanged.connect(
partial(self.updateLabel,
header="z",
label=self.pose_z,
slider=self.height_slider))
self.pose_command_layout.addWidget(self.pose_x, 0, 0)
self.pose_command_layout.addWidget(self.pose_y, 0, 1)
self.pose_command_layout.addWidget(self.pose_z, 0, 2)
## Button to send the pose command to state machine as poseyaw event
self.send_pose_command_button = QPushButton("Send Pose Command")
self.send_pose_command_button.clicked.connect(
self.poseCommandButtonCallback)
self.pose_command_layout.addWidget(self.send_pose_command_button, 0, 3)
self._additional_commands_layout.addWidget(self.pose_command_container)
## Pose command container to store pose from Rviz and send to state
## machine
self.pose_command = None
## Sliders for setting vx,vy,vz, yaw
self.velocity_sliders = []
## Labels vx,vy,vz,yaw as an array
self.velocity_command_labels = []
self._additional_commands_layout.addWidget(
QLabel('Velocity Command (m/s), Yaw (deg)'))
for i in range(3):
self.velocity_sliders.append(
self.createSlider(-20.0, 20.0, 0.0, 1.0))
self._additional_commands_layout.addWidget(
self.velocity_sliders[i])
# Slider for yaw
self.velocity_sliders.append(
self.createSlider(-180.0, 180.0, 0.0, 10.0))
self._additional_commands_layout.addWidget(self.velocity_sliders[-1])
## Add button for triggering velocity yaw
self.velocity_command_container = QWidget()
## Velocity command layout
self.velocity_command_layout = QGridLayout()
self.velocity_command_container.setLayout(self.velocity_command_layout)
for i, axis_label in enumerate(['x', 'y', 'z']):
# label to display velocity command from rviz to user
self.velocity_command_labels.append(
QLabel("v{0}: {1:.2f}".format(
axis_label, self.velocity_sliders[i].value() / 10.0)))
self.velocity_sliders[i].valueChanged.connect(
partial(self.updateLabel,
header="v" + str(i),
label=self.velocity_command_labels[i],
slider=self.velocity_sliders[i],
scale=10.0))
self.velocity_command_layout.addWidget(
self.velocity_command_labels[i], 0, i)
# Label for yaw
self.velocity_command_labels.append(
QLabel("Yaw: {0:.2f}".format(self.velocity_sliders[-1].value())))
self.velocity_sliders[-1].valueChanged.connect(
partial(self.updateLabel,
header="Yaw",
label=self.velocity_command_labels[-1],
slider=self.velocity_sliders[-1]))
self.velocity_command_layout.addWidget(
self.velocity_command_labels[-1], 0, 3)
## Button to send the pose command to state machine as poseyaw event
self.send_velocity_command_button = QPushButton(
"Send Velocity Command")
self.send_velocity_command_button.clicked.connect(
self.velocityCommandButtonCallback)
self.velocity_command_layout.addWidget(
self.send_velocity_command_button, 0, 4)
self._additional_commands_layout.addWidget(
self.velocity_command_container)
self._additional_commands_layout.addStretch()
self._container.addTab(self._status_tab, "StatusBasicCommands")
self._container.addTab(self._additional_commands_tab,
"AdditionalCommands")
context.add_widget(self._container)
# Add textboxes to update hooks from eventTrigger class
# Define Partial callbacks
systemStatusCallback = partial(self.updateStatus,
text_box=self.system_status_textbox)
# Connect Event Triggers
self.event_trigger.status_signal.connect(systemStatusCallback)
self.event_trigger.pose_command_signal.connect(
self.poseCommandCallback)
def createSlider(self, minimum, maximum, default_value, tick_interval):
"""
Create a QtSlider with specified properties
Parameters:
@param minimum Minimum value for slider
@param maximum Maximum value for slider
@param default_value Initial value the slider is set to
@param tick_inverval Integer value specifying difference between
successive ticks
@return Slider value
"""
slider = QSlider(Qt.Horizontal)
slider.setMinimum(minimum)
slider.setMaximum(maximum)
slider.setValue(default_value)
slider.setTickPosition(QSlider.TicksBelow)
slider.setTickInterval(tick_interval)
return slider
def _parse_args(self, argv):
"""
Parse extra arguments when plugin is deployed in standalone mode
"""
parser = argparse.ArgumentParser(prog='aerial_autonomy',
add_help=False)
EventTransmissionGUI.add_arguments(parser)
return parser.parse_args(argv)
@staticmethod
def add_arguments(parser):
"""
Notify rqt_gui that this plugin can parse these extra arguments
"""
group = parser.add_argument_group(
'Options for aerial autonomy gui plugin')
group.add_argument("-e",
"--event_file",
type=str,
default='',
help="Event file")
group.add_argument("-c",
"--grid_cols",
type=int,
default=3,
help="Number of columns in grid")
def get_row_col(self, button_index, ncols):
"""
Automatically find the row and col to add the button
to in a grid based on index of the button
"""
col_index = button_index % ncols
row_index = int((button_index - col_index) / ncols)
return (row_index, col_index)
def poseCommandCallback(self, pose):
"""
Saves pose command and updates command display
"""
self.pose_command = pose
self.pose_x.setText("x: {0:.2f}".format(
self.pose_command.pose.position.x))
self.pose_y.setText("y: {0:.2f}".format(
self.pose_command.pose.position.y))
def poseCommandButtonCallback(self):
"""
Publishes stored pose command after setting height from slider
"""
if self.pose_command:
self.pose_command.pose.position.z = self.height_slider.value()
self.event_trigger.triggerPoseCommand(self.pose_command)
# Reset pose command to avoid accidental triggering
self.pose_command = None
self.pose_x.setText('x: -')
self.pose_y.setText('y: -')
else:
print "No pose command to trigger"
def velocityCommandButtonCallback(self):
"""
Publishes stored velocity command
"""
velocity_command = VelocityYaw()
velocity_command.vx = self.velocity_sliders[0].value() / 10.0
velocity_command.vy = self.velocity_sliders[1].value() / 10.0
velocity_command.vz = self.velocity_sliders[2].value() / 10.0
velocity_command.yaw = self.velocity_sliders[3].value() * np.pi / 180.0
self.event_trigger.triggerVelocityCommand(velocity_command)
def updateLabel(self, header, label, slider, scale=1):
"""
Updates height label based on slider value
"""
label.setText(header + ": {0:.2f}".format(slider.value() / scale))
def updateStatus(self, status, text_box):
"""
Generic placeholder function to update text box
"""
if not sip.isdeleted(text_box):
text_box.setHtml(status)
doc_size = text_box.document().size()
text_box.setFixedHeight(doc_size.height() + 10)
def setup_group_frame(self, group):
layout = QVBoxLayout()
# grid for buttons for named targets
grid = QGridLayout()
grid.setSpacing(1)
self.button_group = QButtonGroup(self)
row = 0
column = 0
named_targets = self.get_named_targets(group)
for target in named_targets:
button = QPushButton(target)
self.button_group.addButton(button)
grid.addWidget(button, row, column)
column += 1
if column >= self.MAX_COLUMNS:
row += 1
column = 0
self.button_group.buttonClicked.connect(self._handle_button_clicked)
# grid for show joint value and move arm buttons/text field
joint_values_grid = QGridLayout()
joint_values_grid.setSpacing(1)
button_show_joint_values = QPushButton('Current Joint Values')
button_show_joint_values.clicked[bool].connect(
self._handle_show_joint_values_clicked)
line_edit = QLineEdit()
self.text_joint_values.append(line_edit)
button_move_to = QPushButton('Move to')
button_move_to.clicked[bool].connect(self._handle_move_to_clicked)
joint_values_grid.addWidget(button_show_joint_values, 0, 1)
joint_values_grid.addWidget(line_edit, 0, 2)
joint_values_grid.addWidget(button_move_to, 0, 3)
layout.addLayout(grid)
line = QFrame()
line.setFrameShape(QFrame.HLine)
line.setFrameShadow(QFrame.Sunken)
layout.addWidget(line)
layout.addLayout(joint_values_grid)
frame = QFrame()
frame.setLayout(layout)
return frame
def __init__(self):
super(PostProcMainWidget, self).__init__()
# init the GUI components:
self.bagWidget = BagWidget(self)
self.bagFiles = self.bagWidget.getBagFiles()
self.plotWidget = PlotWidget(self.bagFiles, self)
self.__addPlotBtn = QPushButton("Add new Graph")
self.__delPlotBtn = QPushButton("Delete Graph")
self.__qualityBtn = QPushButton("Compute Data Quality")
# connect signals to slots:
self.__addPlotBtn.clicked.connect(self.openNewGraphDialog)
self.__delPlotBtn.clicked.connect(self.openDelGraphDialog)
self.__qualityBtn.clicked.connect(self.openQualityDialog)
# setup the layout:
layout = QGridLayout()
# (rowIndex, rowStretch)
layout.setRowStretch(
0, 0) # controls behavior when changing the window size
# (widget, fromRow, fromColumn, rowSpan, columnSpan)
layout.addWidget(self.bagWidget, 0, 0, 1, 3)
layout.setRowStretch(1, 0)
layout.addWidget(self.__addPlotBtn, 1, 0, 1, 1)
layout.addWidget(self.__delPlotBtn, 1, 1, 1, 1)
layout.addWidget(self.__qualityBtn, 1, 2, 1, 1)
layout.setRowStretch(2, 1)
layout.addWidget(self.plotWidget, 2, 0, 1, 3)
self.setLayout(layout)
def __init__(self, context):
"""
Create Qt GUI using the event file
"""
super(EventTransmissionGUI, self).__init__(context)
self.setObjectName('ManualEventTriggerGUI')
parser = ArgumentParser()
# Add argument(s) to the parser.
args = self._parse_args(context.argv())
## Create Event trigger
self.event_trigger = RosEventTrigger(args.event_file)
## Parent container to store buttons, textboxes
self._container = QTabWidget()
## Tab to display robot system, state machine status
## and command buttons without any arguments
self._status_tab = QWidget()
## Tab to display positionyaw and velocityyaw commands
## and sliders for them
self._additional_commands_tab = QWidget()
# Set title of the parent container window
self._status_tab.setWindowTitle(self.event_trigger.event_manager_name)
## Layout for status tab
self._layout = QVBoxLayout()
self._status_tab.setLayout(self._layout)
# Create Textboxes and add to Layout
self._layout.addWidget(QLabel('State Machine State'))
## Textbox to show sytem status
self.system_status_textbox = QTextEdit()
self.system_status_textbox.setReadOnly(True)
self._layout.addWidget(self.system_status_textbox)
# Define and connect buttons
self._layout.addWidget(QLabel('Event Triggers'))
## Continer to store event triggering buttons
self.button_container = QWidget()
## List of push buttons to trigger events
self.push_buttons = list()
## Layout for the push buttons
self.button_layout = QGridLayout()
self.button_container.setLayout(self.button_layout)
button_index = 0
for event_name in self.event_trigger.event_names_list:
self.push_buttons.append(QPushButton(event_name))
partial_fun = partial(self.event_trigger.triggerEvent,
event_name=event_name)
self.push_buttons[-1].clicked.connect(partial_fun)
row, col = self.get_row_col(button_index, args.grid_cols)
self.button_layout.addWidget(self.push_buttons[-1], row, col)
button_index += 1
self._layout.addWidget(self.button_container)
## Layout for additional commands tab
self._additional_commands_layout = QVBoxLayout()
self._additional_commands_tab.setLayout(
self._additional_commands_layout)
# Create height slider
self._additional_commands_layout.addWidget(
QLabel('Pose Command Height (m)'))
## Height slider to adjust z coordinate for pose command
## \todo Matt: Load slider settings from param file
self.height_slider = self.createSlider(1.0, 20.0, 2.0, 1.0)
self._additional_commands_layout.addWidget(self.height_slider)
## Add button for triggering pose command
## Container for pose event related objects: slide etc
## \todo Matt: Reset slider value based on current quad height
self.pose_command_container = QWidget()
## Pose command layout
self.pose_command_layout = QGridLayout()
self.pose_command_container.setLayout(self.pose_command_layout)
## x pose label to display position command from rviz to user
self.pose_x = QLabel('x: -')
## y pose label to display position command from rviz to user
self.pose_y = QLabel('y: -')
## z pose label to display position command from rviz to user
self.pose_z = QLabel("z: {0:.2f}".format(self.height_slider.value()))
self.height_slider.valueChanged.connect(
partial(self.updateLabel,
header="z",
label=self.pose_z,
slider=self.height_slider))
self.pose_command_layout.addWidget(self.pose_x, 0, 0)
self.pose_command_layout.addWidget(self.pose_y, 0, 1)
self.pose_command_layout.addWidget(self.pose_z, 0, 2)
## Button to send the pose command to state machine as poseyaw event
self.send_pose_command_button = QPushButton("Send Pose Command")
self.send_pose_command_button.clicked.connect(
self.poseCommandButtonCallback)
self.pose_command_layout.addWidget(self.send_pose_command_button, 0, 3)
self._additional_commands_layout.addWidget(self.pose_command_container)
## Pose command container to store pose from Rviz and send to state
## machine
self.pose_command = None
## Sliders for setting vx,vy,vz, yaw
self.velocity_sliders = []
## Labels vx,vy,vz,yaw as an array
self.velocity_command_labels = []
self._additional_commands_layout.addWidget(
QLabel('Velocity Command (m/s), Yaw (deg)'))
for i in range(3):
self.velocity_sliders.append(
self.createSlider(-20.0, 20.0, 0.0, 1.0))
self._additional_commands_layout.addWidget(
self.velocity_sliders[i])
# Slider for yaw
self.velocity_sliders.append(
self.createSlider(-180.0, 180.0, 0.0, 10.0))
self._additional_commands_layout.addWidget(self.velocity_sliders[-1])
## Add button for triggering velocity yaw
self.velocity_command_container = QWidget()
## Velocity command layout
self.velocity_command_layout = QGridLayout()
self.velocity_command_container.setLayout(self.velocity_command_layout)
for i, axis_label in enumerate(['x', 'y', 'z']):
# label to display velocity command from rviz to user
self.velocity_command_labels.append(
QLabel("v{0}: {1:.2f}".format(
axis_label, self.velocity_sliders[i].value() / 10.0)))
self.velocity_sliders[i].valueChanged.connect(
partial(self.updateLabel,
header="v" + str(i),
label=self.velocity_command_labels[i],
slider=self.velocity_sliders[i],
scale=10.0))
self.velocity_command_layout.addWidget(
self.velocity_command_labels[i], 0, i)
# Label for yaw
self.velocity_command_labels.append(
QLabel("Yaw: {0:.2f}".format(self.velocity_sliders[-1].value())))
self.velocity_sliders[-1].valueChanged.connect(
partial(self.updateLabel,
header="Yaw",
label=self.velocity_command_labels[-1],
slider=self.velocity_sliders[-1]))
self.velocity_command_layout.addWidget(
self.velocity_command_labels[-1], 0, 3)
## Button to send the pose command to state machine as poseyaw event
self.send_velocity_command_button = QPushButton(
"Send Velocity Command")
self.send_velocity_command_button.clicked.connect(
self.velocityCommandButtonCallback)
self.velocity_command_layout.addWidget(
self.send_velocity_command_button, 0, 4)
self._additional_commands_layout.addWidget(
self.velocity_command_container)
self._additional_commands_layout.addStretch()
self._container.addTab(self._status_tab, "StatusBasicCommands")
self._container.addTab(self._additional_commands_tab,
"AdditionalCommands")
context.add_widget(self._container)
# Add textboxes to update hooks from eventTrigger class
# Define Partial callbacks
systemStatusCallback = partial(self.updateStatus,
text_box=self.system_status_textbox)
# Connect Event Triggers
self.event_trigger.status_signal.connect(systemStatusCallback)
self.event_trigger.pose_command_signal.connect(
self.poseCommandCallback)
def __init__(self, title, jsp, num_rows=0):
super(JointStatePublisherGui, self).__init__()
self.jsp = jsp
self.joint_map = {}
self.vlayout = QVBoxLayout(self)
self.gridlayout = QGridLayout()
font = QFont("Helvetica", 9, QFont.Bold)
### Generate sliders ###
sliders = []
for name in self.jsp.joint_list:
if name not in self.jsp.free_joints:
continue
joint = self.jsp.free_joints[name]
if joint['min'] == joint['max']:
continue
joint_layout = QVBoxLayout()
row_layout = QHBoxLayout()
label = QLabel(name)
label.setFont(font)
row_layout.addWidget(label)
display = QLineEdit("0.00")
display.setAlignment(Qt.AlignRight)
display.setFont(font)
display.setReadOnly(True)
row_layout.addWidget(display)
joint_layout.addLayout(row_layout)
slider = QSlider(Qt.Horizontal)
slider.setFont(font)
slider.setRange(0, RANGE)
slider.setValue(RANGE/2)
joint_layout.addWidget(slider)
self.joint_map[name] = {'slidervalue': 0, 'display': display,
'slider': slider, 'joint': joint}
# Connect to the signal provided by QSignal
slider.valueChanged.connect(self.onValueChanged)
sliders.append(joint_layout)
# Determine number of rows to be used in grid
self.num_rows = num_rows
# if desired num of rows wasn't set, default behaviour is a vertical layout
if self.num_rows == 0:
self.num_rows = len(sliders) # equals VBoxLayout
# Generate positions in grid and place sliders there
self.positions = self.generate_grid_positions(len(sliders), self.num_rows)
for item, pos in zip(sliders, self.positions):
self.gridlayout.addLayout(item, *pos)
# Set zero positions read from parameters
self.center()
# Synchronize slider and displayed value
self.sliderUpdate(None)
# Set up a signal for updating the sliders based on external joint info
self.sliderUpdateTrigger.connect(self.updateSliders)
self.vlayout.addLayout(self.gridlayout)
# Buttons for randomizing and centering sliders and
# Spinbox for on-the-fly selecting number of rows
self.randbutton = QPushButton('Randomize', self)
self.randbutton.clicked.connect(self.randomize_event)
self.vlayout.addWidget(self.randbutton)
self.ctrbutton = QPushButton('Center', self)
self.ctrbutton.clicked.connect(self.center_event)
self.vlayout.addWidget(self.ctrbutton)
self.maxrowsupdown = QSpinBox()
self.maxrowsupdown.setMinimum(1)
self.maxrowsupdown.setMaximum(len(sliders))
self.maxrowsupdown.setValue(self.num_rows)
self.maxrowsupdown.lineEdit().setReadOnly(True) # don't edit it by hand to avoid weird resizing of window
self.maxrowsupdown.valueChanged.connect(self.reorggrid_event)
self.vlayout.addWidget(self.maxrowsupdown)
class JointStatePublisherGui(QWidget):
sliderUpdateTrigger = Signal()
def __init__(self, title, jsp, num_rows=0):
super(JointStatePublisherGui, self).__init__()
self.jsp = jsp
self.joint_map = {}
self.vlayout = QVBoxLayout(self)
self.scrollable = QWidget()
self.gridlayout = QGridLayout()
self.scroll = QScrollArea()
self.scroll.setWidgetResizable(True)
font = QFont("Helvetica", 9, QFont.Bold)
### Generate sliders ###
sliders = []
for name in self.jsp.joint_list:
if name not in self.jsp.free_joints:
continue
joint = self.jsp.free_joints[name]
if joint['min'] == joint['max']:
continue
joint_layout = QVBoxLayout()
row_layout = QHBoxLayout()
label = QLabel(name)
label.setFont(font)
row_layout.addWidget(label)
display = QLineEdit("0.00")
display.setAlignment(Qt.AlignRight)
display.setFont(font)
display.setReadOnly(True)
row_layout.addWidget(display)
joint_layout.addLayout(row_layout)
slider = QSlider(Qt.Horizontal)
slider.setFont(font)
slider.setRange(0, RANGE)
slider.setValue(RANGE / 2)
joint_layout.addWidget(slider)
self.joint_map[name] = {
'slidervalue': 0,
'display': display,
'slider': slider,
'joint': joint
}
# Connect to the signal provided by QSignal
slider.valueChanged.connect(
lambda event, name=name: self.onValueChangedOne(name))
sliders.append(joint_layout)
# Determine number of rows to be used in grid
self.num_rows = num_rows
# if desired num of rows wasn't set, default behaviour is a vertical layout
if self.num_rows == 0:
self.num_rows = len(sliders) # equals VBoxLayout
# Generate positions in grid and place sliders there
self.positions = self.generate_grid_positions(len(sliders),
self.num_rows)
for item, pos in zip(sliders, self.positions):
self.gridlayout.addLayout(item, *pos)
# Set zero positions read from parameters
self.center()
# Synchronize slider and displayed value
self.sliderUpdate(None)
# Set up a signal for updating the sliders based on external joint info
self.sliderUpdateTrigger.connect(self.updateSliders)
self.scrollable.setLayout(self.gridlayout)
self.scroll.setWidget(self.scrollable)
self.vlayout.addWidget(self.scroll)
# Buttons for randomizing and centering sliders and
# Spinbox for on-the-fly selecting number of rows
self.randbutton = QPushButton('Randomize', self)
self.randbutton.clicked.connect(self.randomize_event)
self.vlayout.addWidget(self.randbutton)
self.ctrbutton = QPushButton('Center', self)
self.ctrbutton.clicked.connect(self.center_event)
self.vlayout.addWidget(self.ctrbutton)
self.maxrowsupdown = QSpinBox()
self.maxrowsupdown.setMinimum(1)
self.maxrowsupdown.setMaximum(len(sliders))
self.maxrowsupdown.setValue(self.num_rows)
self.maxrowsupdown.valueChanged.connect(self.reorggrid_event)
self.vlayout.addWidget(self.maxrowsupdown)
self.setLayout(self.vlayout)
def onValueChangedOne(self, name):
# A slider value was changed, but we need to change the joint_info metadata.
joint_info = self.joint_map[name]
joint_info['slidervalue'] = joint_info['slider'].value()
joint = joint_info['joint']
joint['position'] = self.sliderToValue(joint_info['slidervalue'],
joint)
joint_info['display'].setText("%.2f" % joint['position'])
@pyqtSlot()
def updateSliders(self):
self.update_sliders()
def update_sliders(self):
for name, joint_info in self.joint_map.items():
joint = joint_info['joint']
joint_info['slidervalue'] = self.valueToSlider(
joint['position'], joint)
joint_info['slider'].setValue(joint_info['slidervalue'])
def center_event(self, event):
self.center()
def center(self):
rospy.loginfo("Centering")
for name, joint_info in self.joint_map.items():
joint = joint_info['joint']
joint_info['slider'].setValue(
self.valueToSlider(joint['zero'], joint))
def reorggrid_event(self, event):
self.reorganize_grid(event)
def reorganize_grid(self, number_of_rows):
self.num_rows = number_of_rows
# Remove items from layout (won't destroy them!)
items = []
for pos in self.positions:
item = self.gridlayout.itemAtPosition(*pos)
items.append(item)
self.gridlayout.removeItem(item)
# Generate new positions for sliders and place them in their new spots
self.positions = self.generate_grid_positions(len(items),
self.num_rows)
for item, pos in zip(items, self.positions):
self.gridlayout.addLayout(item, *pos)
def generate_grid_positions(self, num_items, num_rows):
if num_rows == 0:
return []
positions = [
(y, x)
for x in range(int((math.ceil(float(num_items) / num_rows))))
for y in range(num_rows)
]
positions = positions[:num_items]
return positions
def randomize_event(self, event):
self.randomize()
def randomize(self):
rospy.loginfo("Randomizing")
for name, joint_info in self.joint_map.items():
joint = joint_info['joint']
joint_info['slider'].setValue(
self.valueToSlider(random.uniform(joint['min'], joint['max']),
joint))
def sliderUpdate(self, event):
for name, joint_info in self.joint_map.items():
joint_info['slidervalue'] = joint_info['slider'].value()
self.update_sliders()
def valueToSlider(self, value, joint):
return (value - joint['min']) * float(RANGE) / (joint['max'] -
joint['min'])
def sliderToValue(self, slider, joint):
pctvalue = slider / float(RANGE)
return joint['min'] + (joint['max'] - joint['min']) * pctvalue
def __init__(self, content):
super(SliderPublisher, self).__init__()
content = content.replace('\t', ' ')
# get message types
self.publishers = {}
self.values = {}
pkgs = []
# to keep track of key ordering in the yaml file
order = []
old = []
for topic, info in yaml.load(content).items():
pkg, msg = info['type'].split('/')
pkgs.append(__import__(pkg, globals(), locals(), ['msg']))
self.publishers[topic] = Publisher(topic,
getattr(pkgs[-1].msg,
msg), info)
order.append((topic, []))
for key in info:
self.values[key] = info[key]
order[-1][1].append((content.find(' ' + key + ':'), key))
old.append((content.find(' ' + key + ':'), key))
for bound in ['min', 'max']:
self.values[key][bound] = float(self.values[key][bound])
self.values[key]['val'] = 0
order[-1][1].sort()
order.sort(key=lambda x: x[1][0][0])
# build sliders - thanks joint_state_publisher
sliderUpdateTrigger = Signal()
self.vlayout = QVBoxLayout(self)
self.gridlayout = QGridLayout()
font = QFont("Helvetica", 9, QFont.Bold)
topic_font = QFont("Helvetica", 10, QFont.Bold)
sliders = []
self.key_map = {}
y = 0
for topic, keys in order:
topic_layout = QVBoxLayout()
label = QLabel(topic)
label.setFont(topic_font)
topic_layout.addWidget(label)
self.gridlayout.addLayout(topic_layout, *(y, 0))
y += 1
for idx, key in keys:
key_layout = QVBoxLayout()
row_layout = QHBoxLayout()
label = QLabel(key)
label.setFont(font)
row_layout.addWidget(label)
display = QLineEdit("0.00")
display.setAlignment(Qt.AlignRight)
display.setFont(font)
display.setReadOnly(True)
row_layout.addWidget(display)
key_layout.addLayout(row_layout)
slider = QSlider(Qt.Horizontal)
slider.setFont(font)
slider.setRange(0, RANGE)
slider.setValue(RANGE / 2)
key_layout.addWidget(slider)
self.key_map[key] = {
'slidervalue': 0,
'display': display,
'slider': slider
}
slider.valueChanged.connect(self.onValueChanged)
self.gridlayout.addLayout(key_layout, *(y, 0))
y += 1
#sliders.append(key_layout)
# Generate positions in grid and place sliders there
#self.positions = [(y,0) for y in range(len(sliders))]
#for item, pos in zip(sliders, self.positions):
# self.gridlayout.addLayout(item, *pos)
self.vlayout.addLayout(self.gridlayout)
self.ctrbutton = QPushButton('Center', self)
self.ctrbutton.clicked.connect(self.center)
self.vlayout.addWidget(self.ctrbutton)
self.center(1)
class JointStatePublisherGui(QWidget):
sliderUpdateTrigger = Signal()
def __init__(self, title, jsp, num_rows=0):
super(JointStatePublisherGui, self).__init__()
self.jsp = jsp
self.joint_map = {}
self.vlayout = QVBoxLayout(self)
self.gridlayout = QGridLayout()
font = QFont("Helvetica", 9, QFont.Bold)
### Generate sliders ###
sliders = []
for name in self.jsp.joint_list:
if name not in self.jsp.free_joints:
continue
joint = self.jsp.free_joints[name]
if joint['min'] == joint['max']:
continue
joint_layout = QVBoxLayout()
row_layout = QHBoxLayout()
label = QLabel(name)
label.setFont(font)
row_layout.addWidget(label)
display = QLineEdit("0.00")
display.setAlignment(Qt.AlignRight)
display.setFont(font)
display.setReadOnly(True)
row_layout.addWidget(display)
joint_layout.addLayout(row_layout)
slider = QSlider(Qt.Horizontal)
slider.setFont(font)
slider.setRange(0, RANGE)
slider.setValue(RANGE/2)
joint_layout.addWidget(slider)
self.joint_map[name] = {'slidervalue': 0, 'display': display,
'slider': slider, 'joint': joint}
# Connect to the signal provided by QSignal
slider.valueChanged.connect(self.onValueChanged)
sliders.append(joint_layout)
# Determine number of rows to be used in grid
self.num_rows = num_rows
# if desired num of rows wasn't set, default behaviour is a vertical layout
if self.num_rows == 0:
self.num_rows = len(sliders) # equals VBoxLayout
# Generate positions in grid and place sliders there
self.positions = self.generate_grid_positions(len(sliders), self.num_rows)
for item, pos in zip(sliders, self.positions):
self.gridlayout.addLayout(item, *pos)
# Set zero positions read from parameters
self.center()
# Synchronize slider and displayed value
self.sliderUpdate(None)
# Set up a signal for updating the sliders based on external joint info
self.sliderUpdateTrigger.connect(self.updateSliders)
self.vlayout.addLayout(self.gridlayout)
# Buttons for randomizing and centering sliders and
# Spinbox for on-the-fly selecting number of rows
self.randbutton = QPushButton('Randomize', self)
self.randbutton.clicked.connect(self.randomize_event)
self.vlayout.addWidget(self.randbutton)
self.ctrbutton = QPushButton('Center', self)
self.ctrbutton.clicked.connect(self.center_event)
self.vlayout.addWidget(self.ctrbutton)
self.maxrowsupdown = QSpinBox()
self.maxrowsupdown.setMinimum(1)
self.maxrowsupdown.setMaximum(len(sliders))
self.maxrowsupdown.setValue(self.num_rows)
self.maxrowsupdown.lineEdit().setReadOnly(True) # don't edit it by hand to avoid weird resizing of window
self.maxrowsupdown.valueChanged.connect(self.reorggrid_event)
self.vlayout.addWidget(self.maxrowsupdown)
@pyqtSlot(int)
def onValueChanged(self, event):
# A slider value was changed, but we need to change the joint_info metadata.
for name, joint_info in self.joint_map.items():
joint_info['slidervalue'] = joint_info['slider'].value()
joint = joint_info['joint']
joint['position'] = self.sliderToValue(joint_info['slidervalue'], joint)
joint_info['display'].setText("%.2f" % joint['position'])
@pyqtSlot()
def updateSliders(self):
self.update_sliders()
def update_sliders(self):
for name, joint_info in self.joint_map.items():
joint = joint_info['joint']
joint_info['slidervalue'] = self.valueToSlider(joint['position'],
joint)
joint_info['slider'].setValue(joint_info['slidervalue'])
def center_event(self, event):
self.center()
def center(self):
rospy.loginfo("Centering")
for name, joint_info in self.joint_map.items():
joint = joint_info['joint']
joint_info['slider'].setValue(self.valueToSlider(joint['zero'], joint))
def reorggrid_event(self, event):
self.reorganize_grid(event)
def reorganize_grid(self, number_of_rows):
self.num_rows = number_of_rows
# Remove items from layout (won't destroy them!)
items = []
for pos in self.positions:
item = self.gridlayout.itemAtPosition(*pos)
items.append(item)
self.gridlayout.removeItem(item)
# Generate new positions for sliders and place them in their new spots
self.positions = self.generate_grid_positions(len(items), self.num_rows)
for item, pos in zip(items, self.positions):
self.gridlayout.addLayout(item, *pos)
def generate_grid_positions(self, num_items, num_rows):
if num_rows==0:
return []
positions = [(y, x) for x in range(int((math.ceil(float(num_items) / num_rows)))) for y in range(num_rows)]
positions = positions[:num_items]
return positions
def randomize_event(self, event):
self.randomize()
def randomize(self):
rospy.loginfo("Randomizing")
for name, joint_info in self.joint_map.items():
joint = joint_info['joint']
joint_info['slider'].setValue(
self.valueToSlider(random.uniform(joint['min'], joint['max']), joint))
def sliderUpdate(self, event):
for name, joint_info in self.joint_map.items():
joint_info['slidervalue'] = joint_info['slider'].value()
self.update_sliders()
def valueToSlider(self, value, joint):
return (value - joint['min']) * float(RANGE) / (joint['max'] - joint['min'])
def sliderToValue(self, slider, joint):
pctvalue = slider / float(RANGE)
return joint['min'] + (joint['max']-joint['min']) * pctvalue
class SliderPublisher(QWidget):
def __init__(self, content):
super(SliderPublisher, self).__init__()
content = content.replace('\t', ' ')
# get message types
self.publishers = {}
self.values = {}
pkgs = []
# to keep track of key ordering in the yaml file
order = []
old = []
for topic, info in yaml.load(content).items():
pkg, msg = info['type'].split('/')
pkgs.append(__import__(pkg, globals(), locals(), ['msg']))
self.publishers[topic] = Publisher(topic,
getattr(pkgs[-1].msg,
msg), info)
order.append((topic, []))
for key in info:
self.values[key] = info[key]
order[-1][1].append((content.find(' ' + key + ':'), key))
old.append((content.find(' ' + key + ':'), key))
for bound in ['min', 'max']:
self.values[key][bound] = float(self.values[key][bound])
self.values[key]['val'] = 0
order[-1][1].sort()
order.sort(key=lambda x: x[1][0][0])
# build sliders - thanks joint_state_publisher
sliderUpdateTrigger = Signal()
self.vlayout = QVBoxLayout(self)
self.gridlayout = QGridLayout()
font = QFont("Helvetica", 9, QFont.Bold)
topic_font = QFont("Helvetica", 10, QFont.Bold)
sliders = []
self.key_map = {}
y = 0
for topic, keys in order:
topic_layout = QVBoxLayout()
label = QLabel(topic)
label.setFont(topic_font)
topic_layout.addWidget(label)
self.gridlayout.addLayout(topic_layout, *(y, 0))
y += 1
for idx, key in keys:
key_layout = QVBoxLayout()
row_layout = QHBoxLayout()
label = QLabel(key)
label.setFont(font)
row_layout.addWidget(label)
display = QLineEdit("0.00")
display.setAlignment(Qt.AlignRight)
display.setFont(font)
display.setReadOnly(True)
row_layout.addWidget(display)
key_layout.addLayout(row_layout)
slider = QSlider(Qt.Horizontal)
slider.setFont(font)
slider.setRange(0, RANGE)
slider.setValue(RANGE / 2)
key_layout.addWidget(slider)
self.key_map[key] = {
'slidervalue': 0,
'display': display,
'slider': slider
}
slider.valueChanged.connect(self.onValueChanged)
self.gridlayout.addLayout(key_layout, *(y, 0))
y += 1
#sliders.append(key_layout)
# Generate positions in grid and place sliders there
#self.positions = [(y,0) for y in range(len(sliders))]
#for item, pos in zip(sliders, self.positions):
# self.gridlayout.addLayout(item, *pos)
self.vlayout.addLayout(self.gridlayout)
self.ctrbutton = QPushButton('Center', self)
self.ctrbutton.clicked.connect(self.center)
self.vlayout.addWidget(self.ctrbutton)
self.center(1)
def sliderToValue(self, slider, key):
val = self.values[key]
return val['min'] + slider * (val['max'] - val['min']) / RANGE
@pyqtSlot(int)
def onValueChanged(self, event):
# A slider value was changed, but we need to change the joint_info metadata.
for key, key_info in self.key_map.items():
key_info['slidervalue'] = key_info['slider'].value()
# build corresponding value
self.values[key]['val'] = self.sliderToValue(
key_info['slidervalue'], key)
key_info['display'].setText("%.2f" % self.values[key]['val'])
def center(self, event):
for key, key_info in self.key_map.items():
key_info['slider'].setValue(RANGE / 2)
self.onValueChanged(event)
def loop(self):
rate = rospy.Rate(10)
while not rospy.is_shutdown():
for pub in self.publishers:
self.publishers[pub].update(self.values)
rate.sleep()
def __init__(self, context):
super(Supervision, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('Supervision')
while not rospy.has_param(Supervision.StepByStepParam):
rospy.sleep(0.1)
self.step_publisher = rospy.Publisher(Supervision.StepTopic,
EmptyMsg,
queue_size=1)
self.path_execution_publisher = rospy.Publisher(
Supervision.PathExecutionTopic, UInt32, queue_size=1)
# Create QWidget
self._widget = QWidget()
self._layout = QGridLayout(self._widget)
self._layout.setColumnStretch(0, 1)
self._layout.setColumnStretch(3, 1)
def add_space(row):
spacer = QFrame()
spacer.setFrameShape(QFrame.HLine)
#self._layout.addWidget (spacer, row, 0, 1, 4)
self._layout.addWidget(spacer, row, 1, 1, 2)
row = 0
# Step by step
self._layout.addWidget(QLabel("Step by step"), row, 1, 1, 2,
Qt.AlignCenter)
row += 1
step_by_step_spin_box = QSpinBox()
step_by_step_spin_box.setRange(0, 4)
step_by_step_spin_box.setToolTip(
"""Set the step by step level, from 0 (non-stop) to 4 (stop very often)."""
)
step_by_step_spin_box.setValue(
rospy.get_param(Supervision.StepByStepParam))
# step_by_step_spin_box.connect (self.setStepByStepParam)
step_by_step_spin_box.valueChanged.connect(
lambda val: rospy.set_param(Supervision.StepByStepParam, val))
self._layout.addWidget(QLabel("Level: "), row, 1, Qt.AlignRight)
self._layout.addWidget(step_by_step_spin_box, row, 2)
row += 1
self._one_step_button = QPushButton("Execute one step")
self._one_step_button.clicked.connect(
lambda x: self.step_publisher.publish())
self._layout.addWidget(self._one_step_button, row, 2)
row += 1
# Spacer
add_space(row)
row += 1
## Path execution
self._layout.addWidget(QLabel("Path execution"), row, 1, 1, 2,
Qt.AlignCenter)
row += 1
self.path_index_spin_box = QSpinBox()
# step_by_step_spin_box.setRange(0,100000)
execute_path = QPushButton("Execute path")
execute_path.clicked.connect(
lambda unused: self.path_execution_publisher.publish(
self.path_index_spin_box.value()))
self._layout.addWidget(self.path_index_spin_box, row, 1)
self._layout.addWidget(execute_path, row, 2)
row += 1
# Spacer
add_space(row)
row += 1
## Tools
self._layout.addWidget(QLabel("Tools"), row, 1, 1, 2, Qt.AlignCenter)
row += 1
publish_state = QPushButton("Request SoT to publish its state")
publish_state.clicked.connect(lambda u: self.publishState())
self._layout.addWidget(publish_state, row, 2)
row += 1
#self._layout.addWidget (None, row, 0, 1, 4)
# Give QObjects reasonable names
self._widget.setObjectName('SupervisionUi')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). This is useful when you open multiple
# plugins at once. Also if you open multiple instances of your
# plugin at once, these lines add number to make it easy to
# tell from pane to pane.
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
class DiffTab(QWidget):
def __init__(self, bagFiles, parent=None):
super(DiffTab, self).__init__()
self.parent = parent
self.layout = QGridLayout()
self.bagFiles = bagFiles
self.selectedValue = ('', '') # contains value as a tupel like ('<message>', '<value>')
# init the widgets
self.thresholdSetter = ThresholdSetter(self)
self.valueWidget = ValueSelectorWidget(self, False)
self.idSelector = IDSelectorWidget()
self.idSelector.setTitle("Select GT-ObjectID")
# connect the signals to the slots)
self.valueWidget.valueTreeWidget.itemClicked.connect(self.valueSelected)
#layout
self.layout.addWidget(self.thresholdSetter, 0, 0)
self.layout.addWidget(self.valueWidget, 0, 1)
self.layout.addWidget(self.idSelector, 0, 2)
self.setLayout(self.layout)
def valueSelected(self, item):
'''
is called when the tree in the value selector widget is clicked
the id selector should be disabled if the item "object_count" is clicked
'''
if item.text(0) == "object_count":
self.idSelector.setEnabled(False)
self.thresholdSetter.setEnabled(False)
else:
self.idSelector.setEnabled(True)
self.thresholdSetter.setEnabled(True)
# init the idSelector
if self.bagFiles[0] == '' or self.bagFiles[1] == '':
message_module.showMessage("Bag file missing! Please import bag file in the main interface.")
else:
try:
self.idSelector.refreshList(self.bagFiles[0])
except:
message_module.showMessage("Object_IDs could not be parsed. Maybe there is a problem with the selected bag file.")
def getPlotData(self):
'''
provides the data to plot when the user presses the start button
in the plot dialog
'''
# info needed for the legend
plotInfo = {
'label' : '',
'y_label' : '',
'bag' : 1
}
# check if two bag files are imported
for bag in self.bagFiles:
if bag == "":
raise Exception("Bag file missing! Please import bag file in the main interface.")
# get the selected threshold value:
threshold = self.thresholdSetter.getThreshold()
# get the selected category and attribute of the object list message:
selectedValue = self.valueWidget.getCatAndAtt()
category = selectedValue['category']
attribute = selectedValue['attribute']
# check whether attribute is empty
# show error message if it is the case
# and return
if attribute == "":
raise Exception("Please select a plottable attribute.")
# attribute is not empty:
# this part is not used:
### ---------------------------
if attribute == "object_count":
# get object_counts per frame and build the difference
try:
time_list_gt, obj_count_list_gt = Rosbag_Analysis.getObjectCountPerFrame(self.bagFiles[0])
time_list_cam, obj_count_list_cam = Rosbag_Analysis.getObjectCountPerFrame(self.bagFiles[1])
except:
raise Exception("Sorry, unexpected error occurred.")
plotData = (time_list_gt, (obj_count_list_gt - obj_count_list_cam))
plotInfo['label'] = 'difference' + '.'
plotInfo['label'] += 'object_count'
### ----------------------------
# selected attribute is from object_list_msg
else:
try:
obj_id = self.idSelector.getID()
except Exception:
raise Exception("ObjectID is not in the list! Insert valid ID.")
try:
plotData = Rosbag_Analysis.getAdvancedData(self.bagFiles[0], self.bagFiles[1], obj_id, category, attribute, 'difference', threshold)
except ValueError:
raise Exception("Sorry, unexpected error occurred.")
plotInfo['label'] = 'difference' + '.'
plotInfo['label'] += category + '.'
plotInfo['label'] += attribute
if object_list_msg.units.has_key(attribute):
plotInfo['label'] += object_list_msg.units[attribute]
plotInfo['y_label'] = object_list_msg.values_units[attribute]
return plotData, plotInfo
class Dashboard(QWidget):
_scan_button_led = Signal(bool)
_cancel_button_led = Signal(bool)
def __init__(self):
super(Dashboard, self).__init__()
not_latched = False
# latched = True
self.publishers = py_trees_ros.utilities.Publishers([
('scan', "~scan", std_msgs.Empty, not_latched, 1),
('cancel', "~cancel", std_msgs.Empty, not_latched, 1),
])
self.scan_push_button = QPushButton("Scan")
self.scan_push_button.setStyleSheet("QPushButton { font-size: 30pt; }")
self.scan_push_button.setSizePolicy(QSizePolicy.Expanding,
QSizePolicy.Expanding)
self.scan_push_button.pressed.connect(
functools.partial(self.publish_button_message,
self.publishers.scan))
self.cancel_push_button = QPushButton("Cancel")
self.cancel_push_button.setStyleSheet(
"QPushButton { font-size: 30pt; }")
self.cancel_push_button.setSizePolicy(QSizePolicy.Expanding,
QSizePolicy.Expanding)
self.cancel_push_button.pressed.connect(
functools.partial(self.publish_button_message,
self.publishers.cancel))
self.led_strip_flashing = False
self.led_strip_on_count = 1
self.led_strip_colour = "grey"
self.led_strip_lock = threading.Lock()
self.led_strip_timer = QTimer()
self.led_strip_timer.timeout.connect(self.led_strip_timer_callback)
self.led_strip_label = QLabel("LED Strip")
self.led_strip_label.setAlignment(Qt.AlignCenter)
self.led_strip_label.setSizePolicy(QSizePolicy.Expanding,
QSizePolicy.Expanding)
self.led_strip_label.setStyleSheet(
"background-color: %s; font-size: 30pt;" % self.led_strip_colour)
self.hbox_layout = QGridLayout(self)
self.hbox_layout.addWidget(self.scan_push_button)
self.hbox_layout.addWidget(self.cancel_push_button)
self.hbox_layout.addWidget(self.led_strip_label)
self.subscribers = py_trees_ros.utilities.Subscribers([
("report", "/tree/report", std_msgs.String,
self.reality_report_callback),
("led_strip", "/led_strip/display", std_msgs.String,
self.led_strip_display_callback)
])
self.led_strip_timer.start(500) # ms
def publish_button_message(self, publisher):
publisher.publish(std_msgs.Empty())
def reality_report_callback(self, msg):
if msg.data == "cancelling":
self.set_scanning_colour(False)
self.set_cancelling_colour(True)
self.cancel_push_button.setEnabled(True)
elif msg.data == "scanning":
self.set_scanning_colour(True)
self.set_cancelling_colour(False)
self.cancel_push_button.setEnabled(True)
else:
self.set_scanning_colour(False)
self.set_cancelling_colour(False)
self.cancel_push_button.setEnabled(False)
def set_cancelling_colour(self, val):
if val:
self.cancel_push_button.setStyleSheet(
"QPushButton { font-size: 30pt; background-color: red}")
else:
self.cancel_push_button.setStyleSheet(
"QPushButton { font-size: 30pt; }")
def set_scanning_colour(self, val):
if val:
self.scan_push_button.setStyleSheet(
"QPushButton { font-size: 30pt; background-color: green}")
else:
self.scan_push_button.setStyleSheet(
"QPushButton { font-size: 30pt; }")
def led_strip_display_callback(self, msg):
with self.led_strip_lock:
if not msg.data:
self.led_strip_colour = "grey"
self.led_strip_flashing = False
else:
self.led_strip_flashing = True
self.led_strip_colour = None
for colour in ["blue", "red", "green"]:
if colour in msg.data:
self.led_strip_colour = colour
break
if not self.led_strip_colour:
self.led_strip_colour = "pink"
rospy.loginfo(
"Dashboard: received unknown LED colour {0}, setting 'pink'"
.format(msg.data))
@Slot()
def led_strip_timer_callback(self):
with self.led_strip_lock:
if self.led_strip_flashing:
if self.led_strip_on_count > 0:
self.led_strip_on_count = 0
self.led_strip_label.setStyleSheet(
"background-color: none; font-size: 30pt;")
else:
self.led_strip_on_count += 1
self.led_strip_label.setStyleSheet(
"background-color: %s; font-size: 30pt;" %
self.led_strip_colour)
else: # solid
self.led_strip_on_count = 1
self.led_strip_label.setStyleSheet(
"background-color: %s; font-size: 30pt;" %
self.led_strip_colour)
class LoggingWidget(QWidget):
__logging_cb_signal = QtCore.pyqtSignal(String)
__start_logging_signal = QtCore.pyqtSignal()
__stop_logging_signal = QtCore.pyqtSignal()
def __init__(self):
QWidget.__init__(self)
self._status = "unknown"
self._layout = QGridLayout(self)
self._widgets = dict()
self.__logging_cb_signal.connect(self.logging_cb)
self.__start_logging_signal.connect(self.start_logging)
self.__stop_logging_signal.connect(self.stop_logging)
self._add_widget("pid_label", QLabel("PID:", self))
self._add_widget("pid", QLineEdit(self))
self._widgets["pid"].setMaximumWidth(50)
self._add_widget("label_status", QLabel(self))
self._widgets["label_status"].setText("Status: Unknown")
self._widgets["label_status"].setTextFormat(QtCore.Qt.RichText)
self._add_widget("button_start", QPushButton("START", self))
self._add_widget("button_stop", QPushButton("STOP", self))
self._widgets["button_start"].clicked.connect(
self.__start_logging_signal.emit)
self._widgets["button_stop"].clicked.connect(
self.__stop_logging_signal.emit)
self._widgets["label_warning"] = QLabel(self)
self._layout.addWidget(self._widgets["label_warning"], 1, 0, 1, 2)
self._widgets["label_warning"].setTextFormat(QtCore.Qt.RichText)
rospy.Subscriber("/data_logger/status", String,
self.__logging_cb_signal.emit)
self._logging_start = rospy.ServiceProxy('/data_logger/start',
DataLoggerStart)
self._logging_stop = rospy.ServiceProxy('/data_logger/stop',
DataLoggerStop)
def _add_widget(self, name, widget):
""" adds widget to dictionary and puts it in the layout, in order """
self._widgets[name] = widget
self._layout.addWidget(self._widgets[name], 0,
len(self._widgets.keys()) - 1)
def logging_cb(self, data):
self._status = data.data
if data.data == "stopped":
self._widgets["button_stop"].setEnabled(False)
self._widgets["button_start"].setEnabled(True)
self._widgets["label_status"].setText("Status: %s" % data.data)
elif data.data == "started":
self._widgets["button_stop"].setEnabled(True)
self._widgets["button_start"].setEnabled(False)
self._widgets["label_status"].setText(
"Status: <span style='color:#00aa00;'>%s</span>" % data.data)
def start_logging(self):
pid = self._widgets["pid"].text()
log_path = LOG_DEST + "%s" % pid
log_path = os.path.abspath(os.path.expanduser(log_path))
# Make the directory
if not os.path.exists(log_path):
print "Creating destination directory '%s'" % log_path
os.makedirs(log_path)
try:
self._logging_start(log_path, "--all")
except Exception as e:
print "Failed to start logging service:"
print str(e)
def stop_logging(self):
try:
self._logging_stop()
except Exception as e:
print "Failed to stop logging service:"
print str(e)
def __init__(self, node_ns, controller, urdf=None, *args, **kwargs):
super(JointPositionControllerWidget, self).__init__(*args, **kwargs)
self._controller = controller
self._urdf = urdf
loadUi('Controller.ui', self)
self.controller_name.setText(controller.name)
self.controller_type.setText(controller.type)
controller_ns = rospy.resolve_name(controller.name, node_ns)
joint_name_widget = QLabel()
joint_position_widget = QLineEdit()
joint_position_widget.setReadOnly(True)
joint_position_widget.setAlignment(Qt.AlignRight)
joint_position_widget.setText(str(0.0))
extras_layout = QGridLayout()
extras_layout.addWidget(joint_name_widget, 0, 0)
extras_layout.addWidget(joint_position_widget, 0, 1)
extras = QWidget()
extras.setLayout(extras_layout)
self.layout().addWidget(extras)
slider_range = (0, 10000)
limits = {'min': 0.0, 'max': 3.14}
if self._urdf is not None:
joint_name = next(iter([
resource
for interface in controller.claimed_resources
if interface.hardware_interface == \
'hardware_interface::PositionJointInterface'
for resource in interface.resources
]), None)
if joint_name is not None:
joint_name_widget.setText(joint_name)
joint_limit = self._urdf.xpath(
'/robot/joint[@name=$name]/limit', name=joint_name)
if joint_limit:
l = joint_limit[0]
limits = {
'min': float(joint_limit[0].get('lower')),
'max': float(joint_limit[0].get('upper'))
}
pub = rospy.Publisher(controller_ns + '/command',
std_msgs.msg.Float64,
queue_size=10)
def on_value_changed(value):
value = limits['min'] + value / float(
slider_range[1] - slider_range[0]) * (limits['max'] -
limits['min'])
joint_position_widget.setText(str(value))
pub.publish(value)
slider = QSlider(Qt.Horizontal)
slider.setRange(slider_range[0], slider_range[1])
slider.valueChanged.connect(on_value_changed)
extras_layout.addWidget(slider, 1, 0, 1, 2)
class Supervision(Plugin):
StepByStepParam = "/sm_sot_hpp/step_by_step"
StepTopic = "/sm_sot_hpp/step"
PathExecutionTopic = "/sm_sot_hpp/start_path"
PublishStateService = "/sot/publish_state"
def __init__(self, context):
super(Supervision, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('Supervision')
while not rospy.has_param(Supervision.StepByStepParam):
rospy.sleep(0.1)
self.step_publisher = rospy.Publisher(Supervision.StepTopic,
EmptyMsg,
queue_size=1)
self.path_execution_publisher = rospy.Publisher(
Supervision.PathExecutionTopic, UInt32, queue_size=1)
# Create QWidget
self._widget = QWidget()
self._layout = QGridLayout(self._widget)
self._layout.setColumnStretch(0, 1)
self._layout.setColumnStretch(3, 1)
def add_space(row):
spacer = QFrame()
spacer.setFrameShape(QFrame.HLine)
#self._layout.addWidget (spacer, row, 0, 1, 4)
self._layout.addWidget(spacer, row, 1, 1, 2)
row = 0
# Step by step
self._layout.addWidget(QLabel("Step by step"), row, 1, 1, 2,
Qt.AlignCenter)
row += 1
step_by_step_spin_box = QSpinBox()
step_by_step_spin_box.setRange(0, 4)
step_by_step_spin_box.setToolTip(
"""Set the step by step level, from 0 (non-stop) to 4 (stop very often)."""
)
step_by_step_spin_box.setValue(
rospy.get_param(Supervision.StepByStepParam))
# step_by_step_spin_box.connect (self.setStepByStepParam)
step_by_step_spin_box.valueChanged.connect(
lambda val: rospy.set_param(Supervision.StepByStepParam, val))
self._layout.addWidget(QLabel("Level: "), row, 1, Qt.AlignRight)
self._layout.addWidget(step_by_step_spin_box, row, 2)
row += 1
self._one_step_button = QPushButton("Execute one step")
self._one_step_button.clicked.connect(
lambda x: self.step_publisher.publish())
self._layout.addWidget(self._one_step_button, row, 2)
row += 1
# Spacer
add_space(row)
row += 1
## Path execution
self._layout.addWidget(QLabel("Path execution"), row, 1, 1, 2,
Qt.AlignCenter)
row += 1
self.path_index_spin_box = QSpinBox()
# step_by_step_spin_box.setRange(0,100000)
execute_path = QPushButton("Execute path")
execute_path.clicked.connect(
lambda unused: self.path_execution_publisher.publish(
self.path_index_spin_box.value()))
self._layout.addWidget(self.path_index_spin_box, row, 1)
self._layout.addWidget(execute_path, row, 2)
row += 1
# Spacer
add_space(row)
row += 1
## Tools
self._layout.addWidget(QLabel("Tools"), row, 1, 1, 2, Qt.AlignCenter)
row += 1
publish_state = QPushButton("Request SoT to publish its state")
publish_state.clicked.connect(lambda u: self.publishState())
self._layout.addWidget(publish_state, row, 2)
row += 1
#self._layout.addWidget (None, row, 0, 1, 4)
# Give QObjects reasonable names
self._widget.setObjectName('SupervisionUi')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). This is useful when you open multiple
# plugins at once. Also if you open multiple instances of your
# plugin at once, these lines add number to make it easy to
# tell from pane to pane.
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
def publishState(self):
try:
rospy.wait_for_service(Supervision.PublishStateService, 0.5)
publish_state = rospy.ServiceProxy(Supervision.PublishStateService,
EmptySrv)
publish_state()
except rospy.exceptions.ROSException as e:
QMessageBox.warning(self, "Service unreachable.", str(e))
def shutdown_plugin(self):
# TODO unregister all publishers here
pass
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
def __init__(self, title, jsp, num_rows=0):
super(JointStatePublisherGui, self).__init__()
self.jsp = jsp
self.joint_map = {}
self.vlayout = QVBoxLayout(self)
self.scrollable = QWidget()
self.gridlayout = QGridLayout()
self.scroll = QScrollArea()
self.scroll.setWidgetResizable(True)
font = QFont("Helvetica", 9, QFont.Bold)
### Generate sliders ###
sliders = []
for name in self.jsp.joint_list:
if name not in self.jsp.free_joints:
continue
joint = self.jsp.free_joints[name]
if joint['min'] == joint['max']:
continue
joint_layout = QVBoxLayout()
row_layout = QHBoxLayout()
label = QLabel(name)
label.setFont(font)
row_layout.addWidget(label)
display = QLineEdit("0.00")
display.setAlignment(Qt.AlignRight)
display.setFont(font)
display.setReadOnly(True)
row_layout.addWidget(display)
joint_layout.addLayout(row_layout)
slider = QSlider(Qt.Horizontal)
slider.setFont(font)
slider.setRange(0, RANGE)
slider.setValue(RANGE / 2)
joint_layout.addWidget(slider)
self.joint_map[name] = {
'slidervalue': 0,
'display': display,
'slider': slider,
'joint': joint
}
# Connect to the signal provided by QSignal
slider.valueChanged.connect(
lambda event, name=name: self.onValueChangedOne(name))
sliders.append(joint_layout)
# Determine number of rows to be used in grid
self.num_rows = num_rows
# if desired num of rows wasn't set, default behaviour is a vertical layout
if self.num_rows == 0:
self.num_rows = len(sliders) # equals VBoxLayout
# Generate positions in grid and place sliders there
self.positions = self.generate_grid_positions(len(sliders),
self.num_rows)
for item, pos in zip(sliders, self.positions):
self.gridlayout.addLayout(item, *pos)
# Set zero positions read from parameters
self.center()
# Synchronize slider and displayed value
self.sliderUpdate(None)
# Set up a signal for updating the sliders based on external joint info
self.sliderUpdateTrigger.connect(self.updateSliders)
self.scrollable.setLayout(self.gridlayout)
self.scroll.setWidget(self.scrollable)
self.vlayout.addWidget(self.scroll)
# Buttons for randomizing and centering sliders and
# Spinbox for on-the-fly selecting number of rows
self.randbutton = QPushButton('Randomize', self)
self.randbutton.clicked.connect(self.randomize_event)
self.vlayout.addWidget(self.randbutton)
self.ctrbutton = QPushButton('Center', self)
self.ctrbutton.clicked.connect(self.center_event)
self.vlayout.addWidget(self.ctrbutton)
self.maxrowsupdown = QSpinBox()
self.maxrowsupdown.setMinimum(1)
self.maxrowsupdown.setMaximum(len(sliders))
self.maxrowsupdown.setValue(self.num_rows)
self.maxrowsupdown.valueChanged.connect(self.reorggrid_event)
self.vlayout.addWidget(self.maxrowsupdown)
self.setLayout(self.vlayout)
def __init__(self):
super(NodeWidgetsContainer, self).__init__()
yaml_file = rospy.get_param("~config_file")
stream = open(yaml_file, "r")
nodes = yaml.load(stream)
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('mcr_event_gui'), 'ros',
'resources', 'NodeWidgetsContainer.ui')
loadUi(ui_file, self)
grid = QGridLayout()
grid.setSpacing(1)
MAX_COLUMNS = 2
self.setObjectName('NodeWidgetsContainer')
self.clear_button = QPushButton("Clear all")
self.clear_button.clicked[bool].connect(self._handle_clear_clicked)
self.stop_button = QPushButton("Stop all")
self.stop_button.clicked[bool].connect(self._handle_stop_clicked)
grid.addWidget(self.clear_button, 0, 0)
grid.addWidget(self.stop_button, 0, 1)
self.widgets = []
row = 1
column = 0
for k in sorted(nodes.keys()):
node_name = k
widget = NodeEventsWidget(node_name, nodes[k])
width = widget.width()
height = widget.height()
grid.addWidget(widget, row, column)
grid.setColumnMinimumWidth(column, width)
grid.setRowMinimumHeight(row, height)
self.widgets.append(widget)
column += 1
if column >= MAX_COLUMNS:
row += 1
column = 0
self.setLayout(grid)