class beast_settings_gui(Plugin):
def __init__(self, context):
super(beast_settings_gui, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('BEAST Settings')
# Create QWidget
self._widget = QWidget()
ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)),
'beast_settings_gui.ui')
loadUi(ui_file, self._widget)
self._widget.setObjectName('BEAST Settings')
self._widget.setWindowTitle('BEAST Settings')
self.disturbance_type_combo = self._widget.findChild(
QComboBox, 'disturbance_type_combo')
self.disturbance_type_combo.addItems(['No Force', 'Sudden Force'])
self.load_combo = self._widget.findChild(QComboBox, 'load_combo')
self.load_combo.addItems(['0', '9000'])
self.start_already_gripping_combo = self._widget.findChild(
QComboBox, 'start_already_gripping_combo')
self.start_already_gripping_combo.addItems(['False', 'True'])
context.add_widget(self._widget)
self.benchmark_params_service = rospy.Service(
'beast/gui/benchmark_params', BeastBenchmarkParams,
self.benchmark_params_callback)
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 benchmark_params_callback(self, _):
benchmark_params_response = BeastBenchmarkParamsResponse()
benchmark_params_response.disturbance_type = str(
self.disturbance_type_combo.currentText())
benchmark_params_response.load = float(self.load_combo.currentText())
benchmark_params_response.start_already_gripping = self.start_already_gripping_combo.currentText(
) == 'True'
return benchmark_params_response
class MyPlugin(Plugin):
target_pos = []
target_force = []
s = []
s_cmd = []
port = 8002 # where do you expect to get a msg?
bufferSize = 12 # whatever you need
server_thread = []
client_address = ('192.168.0.102', 8000)
def __init__(self, context):
super(MyPlugin, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('MyPlugin')
# Process standalone plugin command-line arguments
from argparse import ArgumentParser
parser = ArgumentParser()
# Add argument(s) to the parser.
parser.add_argument("-q",
"--quiet",
action="store_true",
dest="quiet",
help="Put plugin in silent mode")
args, unknowns = parser.parse_known_args(context.argv())
if not args.quiet:
print 'arguments: ', args
print 'unknowns: ', unknowns
# Create QWidget
self._widget = QWidget()
# Get path to UI file which should be in the "resource" folder of this package
ui_file = os.path.join(rospkg.RosPack().get_path('m3_rqt'), 'resource',
'MyPlugin.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('M3')
# 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)
self.target_pos = self._widget.findChild(QSlider, "target_pos")
self.target_force = self._widget.findChild(QSlider, "target_force")
self.target_pos.valueChanged.connect(self.pos_target_change)
self.target_force.valueChanged.connect(self.force_target_change)
import select, socket
self.s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
self.s.bind(('255.255.255.255', self.port))
self.s.setblocking(0)
self.server_thread = threading.Thread(target=self.receiveStatus)
self.server_thread.daemon = True
self.server_thread.start()
self.s_cmd = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
# thread fuction
def receiveStatus(self):
while not rospy.is_shutdown():
result = select.select([self.s], [], [])
msg = result[0][0].recv(self.bufferSize)
print(unpack('fff', msg))
def shutdown_plugin(self):
# TODO unregister all publishers here
pass
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
#def trigger_configuration(self):
# Comment in to signal that the plugin has a way to configure
# This will enable a setting button (gear icon) in each dock widget title bar
# Usually used to open a modal configuration dialog
def pos_target_change(self):
setpoint = self.target_pos.value()
print(setpoint)
self.s_cmd.sendto(pack('fB', setpoint, 0), self.client_address)
def force_target_change(self):
setpoint = self.target_force.value()
print(setpoint)
self.s_cmd.sendto(pack('fB', setpoint, 1), self.client_address)
class HelpingHandGUI(Plugin):
# Various member variables
# _topic_subscriber = None
# _topic_good = False
# _digitalinput_subscriber = None
# _namespace = None
_available_ns = []
_conf_yaml = None
_trigger_sources = []
_data_buffer = {}
_refresh_data_table_contents_sig = pyqtSignal()
# _pattern = re.compile(r'\/[^/]+\/')
# _digitalinput = None
# _gravcomp_service = None
# _dmp_action_server_client = None
# _topic_name = None
# _num_weights = 0
# _recorded_dmp = None
# _msg_store = None
# _database_service = None
# _dmp_save_name = None
# _pyaudio = None
# _stream = None
# _beep = None
# _joint_space = None
# _cartesian_space = None
# _available_frames = []
# DMP trajectory buffer
_joint_traj = []
# DMP encoding service
_dmp_ecoder = None
# Logging
_init_status_report = {
'status': 'idle',
'last_entry': 'None',
'last_type': 'None',
}
# Logics
_old_gravbutton = 0
_old_rec_button = 0
_recording = False
_gravComp = False
# Some signal
_dmp_recording_started_signal = pyqtSignal()
_dmp_recording_stopped_signal = pyqtSignal()
# TF2
_tf2_buffer = tf2_ros.Buffer()
_tf2_listener = tf2_ros.TransformListener(_tf2_buffer)
_tf_listener = tf.TransformListener()
# Available topics
_available_topics_array = []
_available_topics_dict = dict()
_old_index = 0
def __init__(self, context):
super(HelpingHandGUI, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('HelpingHandGUI')
# Process standalone plugin command-line arguments
from argparse import ArgumentParser
parser = ArgumentParser()
# Add argument(s) to the parser.
parser.add_argument("-q", "--quiet", action="store_true",
dest="quiet",
help="Put plugin in silent mode")
# args, unknowns = parser.parse_known_args(context.argv())
# if not args.quiet:
# rospy.logdebug 'arguments: ', args
# rospy.logdebug 'unknowns: ', unknowns
# Create QWidget
self._widget = QWidget()
# Get path to UI file which should be in the "resource" folder of this package
ui_file = os.path.join(rospkg.RosPack().get_path('helping_hand_gui'), 'qt', 'helpinghand.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
# self._widget.setObjectName('DMPRecordToolGUIUi')
# 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)
# Populate configuration table's headers
self._conf_table = self._widget.findChild(QTableWidget, 'configTable')
self._conf_table.setHorizontalHeaderLabels((
'Trigger Name',
'Robot Namespace',
'Trigger Topic',
'Trigger Type',
'Trigger Value',
'Trigger Callback'))
# Populate data table's headers
self._data_table = self._widget.findChild(QTableWidget, 'data_table')
self._data_table.setHorizontalHeaderLabels((
'Timestamp',
'Trigger Conf',
'Trigger Reason',
'Data Type'))
header = self._data_table.horizontalHeader()
header.setSectionResizeMode(QHeaderView.ResizeToContents)
# Hide the 5th column that contains data identifiers
self._data_table.setColumnCount(self._data_table.columnCount()+1)
self._data_table.setColumnHidden(self._data_table.columnCount()-1, True)
# Connect the refresh signal with the update function
self._refresh_data_table_contents_sig.connect(self._update_data_table)
# Connect the item selected signal to the display details
self._data_table.itemSelectionChanged.connect(self._display_item_details)
# Parse config
self._parse_yaml_config()
# Show conf in table
self._write_conf_to_table()
# Display what we are listening to
self._display_addresses()
# Print initial status report
self.status_report = StatusReport(self._init_status_report, self)
self._print_status()
# DMP Encoding service
self._dmp_ecoder = rospy.ServiceProxy('/encode_traj_to_dmp', EncodeTrajectoryToDMP)
# Save button widget
save_button = self._widget.findChild(QPushButton, 'save_button')
save_button.clicked.connect(partial(self._save_button_pressed, save_button))
# Create database proxy
self._msg_store = MessageStoreProxy()
# Create the service proxies for into saving databse
self._tf_capture_srv = rospy.ServiceProxy('tf_capture', CaptureTF)
self._joint_capture_srv = rospy.ServiceProxy('joint_capture', CaptureJoint)
self._dmp_capture_srv = rospy.ServiceProxy('dmp_capture', CaptureDMP)
# Make sure the services are running !!
try:
self._tf_capture_srv.wait_for_service(0.5)
self._joint_capture_srv.wait_for_service(0.5)
self._dmp_capture_srv.wait_for_service(0.5)
except Exception as e:
rospy.logerr("Could not establish a connection to services. See exception:\n{}".format(e))
exit()
def _print_status(self):
status_text_widget = self._widget.findChild(QTextEdit, 'statusWindow')
status_text = \
"Status: {0}\n"\
"Last saved entry: {1}\n"\
"Last entry type: {2}".format(
self.status_report['status'],
self.status_report['last_entry'],
self.status_report['last_type']
)
status_text_widget.setText(status_text)
# YAML config parser
def _parse_yaml_config(self):
conf_file = os.path.join(rospkg.RosPack().get_path('helping_hand_gui'), 'conf', 'triggers_conf.yml')
self._conf_yaml = yaml.load(open(conf_file), Loader=yaml.FullLoader)
tmp_ns = []
for conf_key, conf_val in self._conf_yaml.items():
tmp_ns.append(conf_val['robot_ns'])
if conf_val['trig_type'] == 'rising_edge':
conf_val['subscriber'] = rospy.Subscriber(conf_val['trig_topic'], Bool, callback=self._rising_edge_cb, callback_args=(conf_key))
elif conf_val['trig_type'] == 'falling_edge':
conf_val['subscriber'] = rospy.Subscriber(conf_val['trig_topic'], Bool, callback=self._falling_edge_cb, callback_args=(conf_key))
elif conf_val['trig_type'] == 'hold':
conf_val['subscriber'] = rospy.Subscriber(conf_val['trig_topic'], Bool, callback=self._hold_cb, callback_args=(conf_key))
else:
self.status_report['status'] = "Error parsing trigger types."
if conf_val['trig_callback'] == 'joint_save':
joint_topic_addr = conf_val['robot_ns'] + conf_val['joint_topic']
conf_val['subscriber'] = rospy.Subscriber(joint_topic_addr, JointState, callback=self._joint_state_cb, callback_args=(conf_key))
if conf_val['trig_callback'] == 'joint_pose_save':
joint_topic_addr = conf_val['robot_ns'] + conf_val['joint_topic']
conf_val['subscriber'] = rospy.Subscriber(joint_topic_addr, JointState, callback=self._joint_state_cb, callback_args=(conf_key))
if conf_val['trig_callback'] == 'joint_dmp_save':
joint_topic_addr = conf_val['robot_ns'] + conf_val['joint_topic']
conf_val['subscriber'] = rospy.Subscriber(joint_topic_addr, JointState, callback=self._joint_state_cb, callback_args=(conf_key))
# Add TF
# Remove duplicate namespaces
self._available_ns = list(dict.fromkeys(tmp_ns))
def _write_conf_to_table(self):
self._conf_table.setRowCount(len(self._conf_yaml))
for row, conf_key in enumerate(self._conf_yaml.items()):
conf = conf_key[1]
self._conf_table.setItem(row, 0, QTableWidgetItem(conf['trig_name']))
self._conf_table.setItem(row, 1, QTableWidgetItem(conf['robot_ns']))
self._conf_table.setItem(row, 2, QTableWidgetItem(conf['trig_topic']))
self._conf_table.setItem(row, 3, QTableWidgetItem(conf['trig_type']))
self._conf_table.setItem(row, 4, QTableWidgetItem(str(conf['trig_value'])))
self._conf_table.setItem(row, 5, QTableWidgetItem(conf['trig_callback']))
def _update_data_table(self):
self._data_table.setRowCount(0)
for data_key, data_dict in self._data_buffer.items():
current_row = self._data_table.rowCount()
self._data_table.insertRow(current_row)
self._data_table.setItem(current_row, 0, QTableWidgetItem(data_dict['time']))
self._data_table.setItem(current_row, 1, QTableWidgetItem(data_dict['trig_name']))
self._data_table.setItem(current_row, 2, QTableWidgetItem(data_dict['trig_type']))
self._data_table.setItem(current_row, 3, QTableWidgetItem(data_dict['data']._type))
self._data_table.setItem(current_row, 4, QTableWidgetItem(data_key))
self._data_table.sortItems(0, Qt.DescendingOrder)
def _display_item_details(self):
text_box = self._widget.findChild(QTextEdit, 'entry_details')
try:
curent_item = self._data_table.currentItem()
current_row = curent_item.row()
entry_identifier = self._data_table.item(current_row, 4).text()
self.status_report['status'] = 'Item selected'
data_details = self._data_buffer[entry_identifier]['data'],
text_box.setText(format(data_details))
except Exception as e:
pass
def _display_addresses(self):
self.discover_available_topics_array()
addr_list_widget = self._widget.findChild(QListWidget, 'addr_list')
for topic in self._available_topics_array:
addr_list_widget.addItem(QListWidgetItem(topic))
# topic = '/ur10_2/joint_states'
# self._available_ns
# Get a list of topics that match a certain type
def discover_available_topics_array(self):
all_topics = rospy.get_published_topics()
for topic, msg_type in all_topics:
if msg_type in DESIRED_MSGS:
self._available_topics_array.append(topic)
self._available_topics_dict[topic] = msg_type
self._available_topics_array.sort()
# The following line removes possible duplicate entries from the array
self._available_topics_array = list(set(self._available_topics_array))
# Discover available robots
def discover_frames(self):
# Define a search pattern, it will look for all namespaces that contain the 'received_control_mode' topic
search_pattern = re.compile(r'^(?!.*link).*')
# search_pattern = re.compile(r'^((?!.*link)|(.*base)).*$')
# Get the list of all currently availabel frames in TF1 (this is a temporary solution until a better one is found)
time.sleep(1) # Give tf time to start working
# all_frames = self._tf_listener.getFrameStrings()
yaml_frames = yaml.load(self._tf2_buffer.all_frames_as_yaml())
# Look through all avaiable topics
for entry in yaml_frames:
namespace = search_pattern.findall(entry)
if len(namespace):
self._available_frames.append(namespace[0])
# Sort the namespaces alphabetically
self._available_frames.sort()
# rospy.logdebug out the result of the search
# rospy.loginfo("Found robots with namespace: \n%s", self._available_frames)
# Add robots namespaces to combobox
self._combobox = self._widget.findChildren(QComboBox, QRegExp('frameSelector'))[0]
self._combobox.insertItems(0, self._available_frames)
self._namespace = self._combobox.currentText()
# Quit if no robots are found
if len(self._available_frames) == 0:
rospy.logerr("[%s]: No robots available !!", rospy.get_name())
sys.exit()
def _save_button_pressed(self, save_button):
save_name_widget = self._widget.findChild(QLineEdit, 'save_name')
save_name = save_name_widget.text()
# Check if a name was entered
if not(len(save_name)):
self.status_report['status'] = 'No name provided !!'
return -1
# Get selected item
curent_item = self._data_table.currentItem()
if curent_item is None:
self.status_report['status'] = 'No item selected !!'
return -1
# Get selected item row
current_row = curent_item.row()
if current_row == -1:
self.status_report['status'] = 'No item selected !!'
return -1
# Get the item identifier and retrieve the data
entry_identifier = self._data_table.item(current_row, 4).text()
data = self._data_buffer[entry_identifier]['data']
# Update the status box
self.status_report['status'] = 'Save button pressed'
rospy.logdebug("Save button pressed !!")
# Handle transform stamped
if data._type == 'geometry_msgs/TransformStamped':
trig_name = self._data_buffer[entry_identifier]['trig_name']
for keys, vals in self._data_buffer.items():
if (vals['trig_name'] == trig_name) and (vals['data']._type == 'sensor_msgs/JointState'):
pass
# Handle Joint space configuration
elif data._type == 'sensor_msgs/JointState':
self._joint_capture_srv.call(data, save_name)
# Handle Joint space configuration
elif data._type == 'robot_module_msgs/JointSpaceDMP':
self._dmp_capture_srv.call(data, CartesianSpaceDMP(), save_name)
# Handle Joint space DMP
# # Saving to database according to the text in the saveName field
# if self._joint_space:
# existingDMP = self._msg_store.query_named(saveName, JointSpaceDMP._type)[0]
# if existingDMP == None:
# rospy.loginfo("Saving new DMP with ID = " + self._dmp_save_name + " to database!")
# self._msg_store.insert_named(saveName, self._recorded_dmp)
# else:
# rospy.loginfo("Updating DMP with ID = " + self._dmp_save_name + " in database!")
# self._msg_store.update_named(saveName, self._recorded_dmp)
# else:
# existingDMP = self._msg_store.query_named(saveName, CartesianSpaceDMP._type)[0]
# if existingDMP == None:
# rospy.loginfo("Saving new DMP with ID = " + self._dmp_save_name + " to database!")
# self._msg_store.insert_named(saveName, self._recorded_dmp)
# else:
# rospy.loginfo("Updating DMP with ID = " + self._dmp_save_name + " in database!")
# self._msg_store.update_named(saveName, self._recorded_dmp)
# Write to report
self.status_report['status'] = 'Data saved'
self.status_report['last_entry'] = save_name
self.status_report['last_type'] = data._type
# Update text fields
save_name_widget.setText('')
details_text_box = self._widget.findChild(QTextEdit, 'entry_details')
details_text_box.setText('')
# Clean up table
self._data_table.clearSelection()
self._data_table.removeRow(current_row)
# # Remove data from buffer
# self._data_buffer.pop(entry_identifier)
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
# Topic subscribers
def _joint_state_cb(self, data, conf_name):
self._conf_yaml[conf_name]['joint_data'] = data
# def save_joint_conf_sig_h(self):
# rospy.loginfo('Saving joint conf !!')
def _rising_edge_cb(self, data, conf_name):
if not(self._conf_yaml[conf_name].has_key('sig_val')):
self._conf_yaml[conf_name]['sig_val'] = data.data
old_val = self._conf_yaml[conf_name]['sig_val']
new_val = data.data
if old_val - new_val == -1:
rospy.logdebug('Rising edge!!')
self._handle_trigger(self._conf_yaml[conf_name])
self._conf_yaml[conf_name]['sig_val'] = new_val
def _falling_edge_cb(self, data, conf_name):
if not(self._conf_yaml[conf_name].has_key('sig_val')):
self._conf_yaml[conf_name]['sig_val'] = data.data
old_val = self._conf_yaml[conf_name]['sig_val']
new_val = data.data
if old_val - new_val == 1:
rospy.logdebug('Falling edge!!')
self._handle_trigger(self._conf_yaml[conf_name])
self._conf_yaml[conf_name]['sig_val'] = new_val
def _hold_cb(self, data, conf_name):
if not('sig_val' in self._conf_yaml[conf_name]):
self._conf_yaml[conf_name]['sig_val'] = data.data
old_val = self._conf_yaml[conf_name]['sig_val']
new_val = data.data
if falling_edge(old_val, new_val):
rospy.logdebug("[_hold_cb] Falling edge")
if len(self._joint_traj):
self._handle_trigger(self._conf_yaml[conf_name])
if rising_edge(old_val, new_val):
pass
rospy.logdebug("[_hold_cb] Rising edge")
if old_val + new_val == 2:
rospy.logdebug("[_hold_cb] Holding")
trig_type = self._conf_yaml[conf_name]['trig_callback']
if trig_type == "joint_dmp_save":
rospy.logdebug("Appending !!")
self._joint_traj.append(self._conf_yaml[conf_name]['joint_data'])
self._conf_yaml[conf_name]['sig_val'] = new_val
def _handle_trigger(self, trigg_conf):
trigg_conf['robot_ns']
trig_type = trigg_conf['trig_callback']
save_data = []
if trig_type == "joint_save":
rospy.logdebug('joint_save')
save_data = [trigg_conf['joint_data']]
elif trig_type == "joint_pose_save":
joint_data = trigg_conf['joint_data']
rospy.logdebug('joint_pose_save')
save_data = [joint_data, TransformStamped()] # , tf_data]
elif trig_type == "joint_dmp_save":
rospy.logdebug('joint_dmp_save')
traj_dur = traj_duration_sec(self._joint_traj)
if traj_dur < 1:
rospy.logwarn("Trajectory too short to store")
else:
dmp_request = EncodeTrajectoryToDMPRequest()
dmp_request.demonstratedTrajectory = self._joint_traj
dmp_request.dmpParameters.N = trigg_conf['joint_dmp_N']
result_dmp = self._dmp_ecoder.call(dmp_request).encodedDMP
save_data = [result_dmp]
self._joint_traj = []
else:
rospy.logerr("Unknown saving type !!")
rospy.logerr("trig_type = {}".format(trig_type))
# self.status_report['status'] = "Error. See terminal"
for data in save_data:
now = datetime.now()
entry_identifier = now.strftime("%H%M%S%f")
current_time = now.strftime("%H:%M:%S")
self._data_buffer[entry_identifier] = {
'data': data,
'time': current_time,
'trig_name': trigg_conf['trig_name'],
'trig_type': trigg_conf['trig_type']
}
self._refresh_data_table_contents_sig.emit()
class SimulationRendererPlugin(Plugin):
"""Basic rqt plugin that allows to start/stop a reload in the renderer."""
def __init__(self, context):
super(SimulationRendererPlugin, self).__init__(context)
self._widget = QWidget()
ui_file = os.path.join(
rospkg.RosPack().get_path("simulation_rqt"), "resource", "SimulationRenderer.ui"
)
loadUi(ui_file, self._widget)
context.add_widget(self._widget)
# Buttons
self.button_stop = self._widget.findChild(QPushButton, "buttonStop")
self.button_reload = self._widget.findChild(QPushButton, "buttonReload")
# GUI Callbacks
self.button_reload.clicked.connect(self.reload_renderer)
self.button_stop.clicked.connect(self.stop_renderer)
groundtruth_topics = self._load_groundtruth_topics()
renderer_topics = groundtruth_topics["renderer"]
self.reload_publisher = rospy.Publisher(
renderer_topics["reload"], EmptyMsg, queue_size=1
)
self.stop_publisher = rospy.Publisher(
renderer_topics["interrupt"], EmptyMsg, queue_size=1
)
self.info_subscriber = rospy.Subscriber(
groundtruth_topics["status"],
GroundtruthStatus,
queue_size=1,
callback=self.receive_groundtruth_status,
)
self.button_reload.setEnabled(True)
self.button_stop.setEnabled(False)
def _load_groundtruth_topics(self):
topic_file = os.path.join(
rospkg.RosPack().get_path("simulation_groundtruth"),
"param",
"groundtruth",
"topics.yaml",
)
with open(topic_file, "r") as file:
return yaml.safe_load(file)
def receive_groundtruth_status(self, msg):
"""Receive new status update from renderer."""
self.button_reload.setEnabled(msg.status == GroundtruthStatus.READY)
self.button_stop.setEnabled(
msg.status == GroundtruthStatus.REMOVE_OLD_TILES
or msg.status == GroundtruthStatus.RENDER_NEW_TILES
)
def reload_renderer(self):
"""Publish message on renderer reload topic."""
self.reload_publisher.publish()
def stop_renderer(self):
"""Publish message on renderer stop topic."""
self.stop_publisher.publish()
class TfEcho(Plugin):
# do_update_label = QtCore.pyqtSignal(String)
def __init__(self, context):
super(TfEcho, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('TfEcho')
rp = rospkg.RosPack()
# Process standalone plugin command-line arguments
from argparse import ArgumentParser
parser = ArgumentParser()
# TODO(lucasw) these aren't working
# parser.add_argument("source_frame", default='') # parent
# parser.add_argument("target_frame", default='') # child
parser.add_argument("-q", "--quiet", action="store_true",
dest="quiet",
help="Put plugin in silent mode")
args, unknowns = parser.parse_known_args(context.argv())
if not args.quiet:
print('arguments: ', args)
print('unknowns: ', unknowns)
self.source_frame = ''
self.target_frame = ''
# self.source_frame = self.args.source_frame
# self.target_frame = self.args.target_frame
# Create QWidget
self._widget = QWidget()
# Get path to UI file which is a sibling of this file
# in this example the .ui and .py file are in the same folder
ui_file = os.path.join(rp.get_path('rqt_tf_echo'), 'resource', 'tf_echo.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('TfEchoUi')
# 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)
cache_time = 10.0
self.tf_buffer = tf2_ros.Buffer(cache_time=rospy.Duration(cache_time))
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
# TODO(lucasw) ros param
self.source_frame = ""
self.target_frame = ""
self.label = {}
self.label['current_time'] = self._widget.findChild(QLabel, 'current_time_label')
self.label['transform_time'] = self._widget.findChild(QLabel, 'transform_time_label')
for axis in ['x', 'y', 'z', 'label']:
self.label['trans_' + axis] = self._widget.findChild(QLabel, 'trans_' + axis + '_label')
self.label['trans_' + axis + '_2'] = self._widget.findChild(QLabel, 'trans_' + axis + '_label_2')
for axis in ['x', 'y', 'z', 'w']:
self.label['quat_' + axis] = self._widget.findChild(QLabel, 'quat_' + axis + '_label')
self.label['quat_' + axis + '_2'] = self._widget.findChild(QLabel, 'quat_' + axis + '_label_2')
for unit in ['rad', 'deg']:
for axis in ['roll', 'pitch', 'yaw', 'label']:
name = 'rot_' + axis + '_' + unit
self.label[name] = self._widget.findChild(QLabel, name + '_label')
self.label[name + '_2'] = self._widget.findChild(QLabel, name + '_label_2')
self.label['source'] = self._widget.findChild(QLineEdit, 'source_line_edit')
self.label['target'] = self._widget.findChild(QLineEdit, 'target_line_edit')
self._widget.setContextMenuPolicy(QtCore.Qt.ActionsContextMenu)
self.actions = {}
self.setup_menu()
self.qt_timer = QTimer()
self.qt_timer.start(200)
self.qt_timer.timeout.connect(self.qt_update)
def add_menu_item(self, menu_name, labels):
menu_text = menu_name
if self.label[labels[0]].isHidden():
menu_text = "Show " + menu_text
else:
menu_text = "Hide " + menu_text
action = QAction(menu_text, self._widget)
action.triggered.connect(partial(self.toggle_labels, labels))
self._widget.addAction(action)
self.actions[menu_name] = action
def setup_menu(self):
for key in self.actions.keys():
self._widget.removeAction(self.actions[key])
self.add_menu_item("transform time", ["transform_time"])
self.add_menu_item("current time", ["current_time"])
self.add_menu_item("source/parent frame", ["source"])
self.add_menu_item("target/child frame", ["target"])
self.add_menu_item("translation", ["trans_x", "trans_y", "trans_z", "trans_label"])
self.add_menu_item("rotation quaternion", ["quat_x", "quat_y", "quat_z", "quat_w"])
self.add_menu_item("rotation (radians)",
["rot_roll_rad", "rot_pitch_rad", "rot_yaw_rad", "rot_label_rad"])
self.add_menu_item("rotation (degrees)",
["rot_roll_deg", "rot_pitch_deg", "rot_yaw_deg", "rot_label_deg"])
def toggle(self, name):
if self.label[name].isHidden():
self.label[name].show()
if name + '_2' in self.label.keys():
self.label[name + '_2'].show()
else:
self.label[name].hide()
if name + '_2' in self.label.keys():
self.label[name + '_2'].hide()
def toggle_labels(self, labels):
for label in labels:
self.toggle(label)
self.setup_menu()
def qt_update(self):
lookup_time = rospy.Time()
cur_time = rospy.Time.now().to_sec()
self.source_frame = self.label['source'].text()
self.target_frame = self.label['target'].text()
# TODO(lucasw) add a text box that can be disabled,
# put any error messages into it.
msg = ''
ts = None
if self.source_frame != "" and self.target_frame != "":
try:
ts = self.tf_buffer.lookup_transform(self.source_frame,
self.target_frame,
lookup_time,
rospy.Duration(0.005))
except tf2.TransformException as ex:
msg = "At time {}, (current time {}) ".format(lookup_time.to_sec(), cur_time)
rospy.logdebug(msg + str(ex))
except rospy.exceptions.ROSTimeMovedBackwardsException as ex:
msg = str(ex)
rospy.logdebug(str(ex))
# update the cur time in case lookup_transform was slow
cur_time = rospy.Time.now().to_sec()
self.label['current_time'].setText("{:1.2f}".format(cur_time))
if ts is None:
# TODO(lucasw) need a dedicated status label in case the user
# hides this?
for label in ['source', 'target']:
self.label[label].setStyleSheet('background-color: yellow')
return
for label in ['source', 'target']:
self.label[label].setStyleSheet('background-color: white')
self.label['transform_time'].setStyleSheet('')
self.label['transform_time'].setText("{:1.2f}".format(ts.header.stamp.to_sec()))
self.label['trans_x'].setText("{:1.3f}".format(ts.transform.translation.x))
self.label['trans_y'].setText("{:1.3f}".format(ts.transform.translation.y))
self.label['trans_z'].setText("{:1.3f}".format(ts.transform.translation.z))
quat = ts.transform.rotation
self.label['quat_x'].setText("{:1.3f}".format(quat.x))
self.label['quat_y'].setText("{:1.3f}".format(quat.y))
self.label['quat_z'].setText("{:1.3f}".format(quat.z))
self.label['quat_w'].setText("{:1.3f}".format(quat.w))
euler = _euler_from_quaternion_msg(quat)
axes = ['roll', 'pitch', 'yaw']
for i in range(3):
self.label['rot_' + axes[i] + '_rad'].setText("{:1.3f}".format(euler[i]))
self.label['rot_' + axes[i] + '_deg'].setText("{:1.3f}".format(math.degrees(euler[i])))
def shutdown_plugin(self):
# TODO unregister all publishers here
pass
def get_param_setting(self, name, label_name, val, instance_settings):
if val == '':
val = rospy.get_param('~' + name, '')
# load from instance_settings only if args/params aren't set
if val == '' and instance_settings.contains(name):
val = instance_settings.value(name)
self.label[label_name].setText(val)
return val
def restore_settings(self, plugin_settings, instance_settings):
self.source_frame = self.get_param_setting("source_frame", "source",
self.source_frame, instance_settings)
self.target_frame = self.get_param_setting("target_frame", "target",
self.target_frame, instance_settings)
for key in self.label.keys():
name = key + '_hidden'
if instance_settings.contains(name):
if instance_settings.value(name) == str(True):
self.label[key].hide()
self.setup_menu()
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value('source_frame', self.source_frame)
instance_settings.set_value('target_frame', self.target_frame)
for key in self.label.keys():
is_hidden = self.label[key].isHidden()
name = key + '_hidden'
instance_settings.set_value(name, str(is_hidden))
class MyPlugin(Plugin):
def __init__(self, context):
super(MyPlugin, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('MyPlugin')
# Process standalone plugin command-line arguments
from argparse import ArgumentParser
parser = ArgumentParser()
# Add argument(s) to the parser.
parser.add_argument("-q",
"--quiet",
action="store_true",
dest="quiet",
help="Put plugin in silent mode")
args, unknowns = parser.parse_known_args(context.argv())
if not args.quiet:
print 'arguments: ', args
print 'unknowns: ', unknowns
# Create QWidget
self._widget = QWidget()
# Get path to UI file which should be in the "resource" folder of this package
ui_file = os.path.join(
rospkg.RosPack().get_path('pressure_controller_setup'), 'resource',
'MyPlugin.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('MyPluginUi')
self.settings = self.get_settings()
self.init_sliders()
#self.init_config()
self._client = actionlib.SimpleActionClient(
'pressure_control', pressure_controller_ros.msg.CommandAction)
self._client_connected = self._client.wait_for_server(
timeout=rospy.rostime.Duration(1))
if not self._client_connected:
print("No command server avaiable... changes will not be sent")
self.set_graph_state(True)
self.send_channel_state(0, self.settings['channel_states'][0])
# 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 init_config(self):
# self._plot_type_index = 0
# self.plot_types = [
# {
# 'title': 'PyQtGraph',
# 'widget_class': PyQtGraphDataPlot,
# 'description': 'Based on PyQtGraph\n- installer: http://luke.campagnola.me/code/pyqtgraph',
# 'enabled': True,#PyQtGraphDataPlot is not None,
# },
# {
# 'title': 'MatPlot',
# 'widget_class': MatDataPlot,
# 'description': 'Based on MatPlotLib\n- needs most CPU\n- needs matplotlib >= 1.1.0\n- if using PySide: PySide > 1.1.0',
# 'enabled': True,#MatDataPlot is not None,
# },
# {
# 'title': 'QwtPlot',
# 'widget_class': QwtDataPlot,
# 'description': 'Based on QwtPlot\n- does not use timestamps\n- uses least CPU\n- needs Python Qwt bindings',
# 'enabled': True,#QwtDataPlot is not None,
# },
# ]
def init_sliders(self):
sliderbox = self._widget.findChild(QLayout, 'Sliders')
graph_button = QPushButton()
graph_button.setCheckable(True)
graph_button.setText("Graph Off")
graph_button.toggle()
graph_button.clicked.connect(self.set_graph_state)
self.graph_button = graph_button
firstCol = QVBoxLayout()
firstCol.addWidget(graph_button)
sliderbox.addLayout(firstCol)
self.sliders = []
all_rows_layout = QVBoxLayout()
chan_idx = 0
for num_channels_row in self.settings['num_channels']:
row_layout = QHBoxLayout()
for i in range(num_channels_row):
idx = chan_idx * 1
slider_group = {
'slider_p': None,
'number_p': None,
'slider_v': None,
'number_v': None,
'on_off': None
}
layout_cluster = QVBoxLayout()
slider_cluster = QHBoxLayout()
label = QLabel()
label.setText("Chan. %d" % (idx + 1))
label.setAlignment(Qt.AlignCenter)
layout_cluster.addWidget(label)
for j in range(2):
layout = QVBoxLayout()
layout.setAlignment(Qt.AlignHCenter)
slider = QSlider(Qt.Vertical)
slider.setMinimum(0)
slider.setMaximum(255)
slider.setValue(
self.settings['valve_offsets'][chan_idx][j])
slider.setTickPosition(QSlider.TicksRight)
slider.setTickInterval(5)
spinbox = QSpinBox()
spinbox.setRange(0, 255)
spinbox.setValue(
self.settings['valve_offsets'][chan_idx][j])
slider.valueChanged.connect(spinbox.setValue)
spinbox.valueChanged.connect(slider.setValue)
cb_function_curr = lambda value, idx=idx: self.send_slider_value(
idx, value)
slider.valueChanged.connect(cb_function_curr)
label = QLabel()
label.setAlignment(Qt.AlignCenter)
if j == 0:
slider_group['slider_p'] = slider
slider_group['number_p'] = spinbox
label.setText("P")
else:
slider_group['slider_v'] = slider
slider_group['number_v'] = spinbox
label.setText("V")
labelmax = QLabel()
labelmax.setAlignment(Qt.AlignCenter)
labelmax.setText("255")
labelmin = QLabel()
labelmin.setAlignment(Qt.AlignCenter)
labelmin.setText("0")
layout.addWidget(label)
layout.addWidget(labelmax)
layout.addWidget(slider, Qt.AlignHCenter)
layout.addWidget(labelmin)
layout.addWidget(spinbox, Qt.AlignHCenter)
layout.setAlignment(slider, Qt.AlignHCenter)
layout.setAlignment(spinbox, Qt.AlignHCenter)
slider_cluster.addLayout(layout)
on_button = QPushButton()
on_button.setCheckable(True)
on_button.setText("Off")
if self.settings['channel_states'][chan_idx]:
on_button.toggle()
on_button.setText("On")
on_button.clicked.connect(
lambda state, idx=idx: self.send_channel_state(idx, state))
slider_group['on_off'] = on_button
layout_cluster.addLayout(slider_cluster)
layout_cluster.addWidget(on_button)
row_layout.addLayout(layout_cluster)
row_layout.addSpacing(20)
self.sliders.append(slider_group)
chan_idx += 1
all_rows_layout.addLayout(row_layout)
sliderbox.addLayout(all_rows_layout)
#self._widget.setLayout(sliderbox)
def get_settings(self):
settings = {}
settings['channel_states'] = rospy.get_param(
'/config_node/channels/states', [1, 1, 1, 1, 1, 1, 1, 1, 1, 1])
settings['num_channels'] = rospy.get_param(
'/pressure_control/num_channels', [])
settings['num_channels_tot'] = sum(settings['num_channels'])
settings['valve_offsets'] = rospy.get_param(
'/config_node/valve_offsets',
[[220, 220]] * settings['num_channels_tot'])
return settings
def send_slider_value(self, idx, value):
v1 = self.sliders[idx]['slider_p'].value()
v2 = self.sliders[idx]['slider_v'].value()
self.settings['valve_offsets'][idx][0] = v1
self.settings['valve_offsets'][idx][1] = v2
self.send_command(command='voffset', args=[idx, v1, v2])
rospy.set_param('/config_node/valve_offsets',
self.settings['valve_offsets'])
def send_channel_state(self, idx, state):
if state:
self.settings['channel_states'][idx] = 1
self.sliders[idx]['on_off'].setText("On")
else:
self.settings['channel_states'][idx] = 0
self.sliders[idx]['on_off'].setText("Off")
self.send_command(command='chan', args=self.settings['channel_states'])
rospy.set_param('/config_node/channels/states',
self.settings['channel_states'])
def set_graph_state(self, value):
if value:
on_off_str = 'on'
self.graph_button.setText("Graph ON")
else:
on_off_str = 'off'
self.graph_button.setText("Graph OFF")
self.send_command(command=on_off_str, args=[])
def send_command(self, command, args, wait_for_ack=False):
if self._client_connected:
# Send commands to the command server and wait for things to be taken care of
goal = pressure_controller_ros.msg.CommandGoal(command=command,
args=args,
wait_for_ack=False)
self._client.send_goal(goal)
self._client.wait_for_result()
if not self._client.get_result():
raise ('Something went wrong and a setting was not validated')
pass
else:
pass
else:
print(command, args)
def shutdown_sliders(self):
print("")
print("Final Valve Offsets:")
print(self.settings['valve_offsets'])
print("")
print("These settings were saved on the pressure controller")
print("")
print(
"To ensure these parameters are always recalled, you can either:")
print(
" 1) Copy these settings into 'valve_offsets' in all your control config files"
)
print(
" 2) Remove the 'valve_offsets' parameter from your control config files"
)
self.set_graph_state(False)
for slider_group in self.sliders:
for widget in slider_group:
if 'on_off' in widget:
slider_group[widget].clicked.disconnect()
else:
slider_group[widget].valueChanged.disconnect()
def save_mcu_settings(self):
self.send_command(command='save', args=[])
def shutdown_plugin(self):
self._client.cancel_all_goals()
self.shutdown_sliders()
self.save_mcu_settings()
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
class MyPlugin(Plugin):
def __init__(self, context):
super(MyPlugin, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('MyPlugin')
# Process standalone plugin command-line arguments
from argparse import ArgumentParser
parser = ArgumentParser()
# Add argument(s) to the parser.
parser.add_argument("-q",
"--quiet",
action="store_true",
dest="quiet",
help="Put plugin in silent mode")
args, unknowns = parser.parse_known_args(context.argv())
if not args.quiet:
print 'arguments: ', args
print 'unknowns: ', unknowns
# Create QWidget
self._widget = QWidget()
# Get path to UI file which should be in the "resource" folder of this package
ui_file = os.path.join(
rospkg.RosPack().get_path('pressure_controller_setup'), 'resource',
'MyPlugin.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('Set Pressures')
self.settings = self.get_settings()
self.init_sliders()
self._client = actionlib.SimpleActionClient(
'pressure_control', pressure_controller_ros.msg.CommandAction)
self._client_connected = self._client.wait_for_server(
timeout=rospy.rostime.Duration(1))
if not self._client_connected:
print("No command server avaiable... changes will not be sent")
self.set_graph_state(True)
self.send_channel_state(0, self.settings['channel_states'][0])
# 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 init_sliders(self):
sliderbox = self._widget.findChild(QLayout, 'Sliders')
firstCol = QVBoxLayout()
graph_button = QPushButton()
graph_button.setCheckable(True)
graph_button.setText("Graph Off")
graph_button.toggle()
graph_button.clicked.connect(self.set_graph_state)
reset_button = QPushButton()
reset_button.setCheckable(False)
reset_button.setText("Reset")
reset_button.clicked.connect(self.set_reset)
self.graph_button = graph_button
self.reset_button = reset_button
firstCol.addWidget(graph_button)
firstCol.addWidget(reset_button)
firstCol.setAlignment(graph_button, Qt.AlignVCenter)
firstCol.setAlignment(reset_button, Qt.AlignVCenter)
zero_button = QPushButton()
zero_button.setCheckable(False)
zero_button.setText("Set All Zero")
zero_button.clicked.connect(self.set_pressure_zero)
self.zero_button = zero_button
firstCol.addWidget(zero_button)
firstCol.setAlignment(zero_button, Qt.AlignVCenter)
transition_box = QVBoxLayout()
label = QLabel()
label.setAlignment(Qt.AlignCenter)
label.setText("Transition Time")
spinbox = QDoubleSpinBox()
spinbox.setMinimum(0)
spinbox.setMaximum(10)
spinbox.setValue(self.settings['transitions'])
spinbox.setDecimals(1)
spinbox.setSingleStep(0.1)
spinbox.setSuffix(" sec")
spinbox.valueChanged.connect(self.set_transition_value)
transition_box.addWidget(label)
transition_box.addWidget(spinbox)
transition_box.setAlignment(label, Qt.AlignBottom)
transition_box.setAlignment(spinbox, Qt.AlignTop)
firstCol.addLayout(transition_box)
self.sliders = []
sliderbox.addLayout(firstCol)
all_rows_layout = QVBoxLayout()
chan_idx = 0
for num_channels_row in self.settings['num_channels']:
row_layout = QHBoxLayout()
for i in range(num_channels_row):
idx = chan_idx * 1
slider_group = {'slider': None, 'number': None, 'on_off': None}
layout_cluster = QVBoxLayout()
layout = QVBoxLayout()
layout.setAlignment(Qt.AlignHCenter)
slider = QSlider(Qt.Vertical)
slider.setMinimum(self.settings['min_pressure'][idx] * 10.0)
slider.setMaximum(self.settings['max_pressure'][idx] * 10.0)
slider.setValue(0)
slider.setTickPosition(QSlider.TicksRight)
slider.setTickInterval(20)
spinbox = QDoubleSpinBox()
spinbox.setMinimum(self.settings['min_pressure'][idx])
spinbox.setMaximum(self.settings['max_pressure'][idx])
spinbox.setValue(0)
spinbox.setDecimals(1)
spinbox.setSingleStep(0.1)
cb_function_curr = lambda value, idx=idx, slider=False: self.send_slider_value(
idx, value, slider)
cb_function_curr2 = lambda value, idx=idx, slider=True: self.send_slider_value(
idx, value, slider)
slider.valueChanged.connect(cb_function_curr2)
spinbox.valueChanged.connect(cb_function_curr)
labelmax = QLabel()
labelmax.setAlignment(Qt.AlignCenter)
labelmax.setText("%0.1f" %
(self.settings['max_pressure'][idx]))
labelmin = QLabel()
labelmin.setAlignment(Qt.AlignCenter)
labelmin.setText("%0.1f" %
(self.settings['min_pressure'][idx]))
layout.addWidget(labelmax)
layout.addWidget(slider)
layout.addWidget(labelmin)
layout.addWidget(spinbox)
layout.setAlignment(slider, Qt.AlignHCenter)
layout.setAlignment(spinbox, Qt.AlignHCenter)
label = QLabel()
label.setText("Chan. %d" % (chan_idx + 1))
label.setAlignment(Qt.AlignCenter)
layout_cluster.addWidget(label)
layout_cluster.addLayout(layout)
slider_group['slider'] = slider
slider_group['number'] = spinbox
on_button = QPushButton()
on_button.setCheckable(True)
on_button.setText("Off")
if self.settings['channel_states'][idx]:
on_button.toggle()
on_button.setText("On")
on_button.clicked.connect(
lambda state, idx=idx: self.send_channel_state(idx, state))
slider_group['on_off'] = on_button
layout_cluster.addWidget(on_button)
row_layout.addLayout(layout_cluster)
row_layout.addSpacing(20)
self.sliders.append(slider_group)
chan_idx += 1
all_rows_layout.addLayout(row_layout)
sliderbox.addLayout(all_rows_layout)
#self._widget.setLayout(sliderbox)
def get_settings(self):
settings = {}
settings['channel_states'] = rospy.get_param(
'/config_node/channels/states', [1, 1, 1, 1, 1, 1, 1, 1, 1, 1])
settings['num_channels'] = rospy.get_param(
'/pressure_control/num_channels', [])
settings['transitions'] = rospy.get_param('/config_node/transitions',
1.0)
maxp = rospy.get_param('/config_node/max_pressure', 40.0)
minp = rospy.get_param('/config_node/min_pressure', -40.0)
settings['num_channels_tot'] = sum(settings['num_channels'])
if isinstance(maxp, list):
settings['max_pressure'] = maxp
elif maxp is not None:
settings['max_pressure'] = [maxp] * settings['num_channels_tot']
if isinstance(minp, list):
settings['min_pressure'] = minp
elif minp is not None:
settings['min_pressure'] = [minp] * settings['num_channels_tot']
settings['pressures'] = [0] * settings['num_channels_tot']
return settings
def set_transition_value(self, value):
self.settings['transitions'] = value
rospy.set_param('/config_node/transitions',
self.settings['transitions'])
def set_pressure_zero(self, value):
self.settings['pressures'] = [0] * self.settings['num_channels_tot']
self.send_command(command='set',
args=[self.settings['transitions']] +
self.settings['pressures'])
for slider_group in self.sliders:
#slider_group['slider'].setValue(0)
slider_group['number'].setValue(0)
def send_slider_value(self, idx, value, slider):
if slider:
press = self.sliders[idx]['slider'].value() / 10.0
self.sliders[idx]['number'].setValue(press)
else:
press = self.sliders[idx]['number'].value()
self.sliders[idx]['slider'].setValue(press * 10.0)
self.settings['pressures'][idx] = press
self.send_command(command='set',
args=[self.settings['transitions']] +
self.settings['pressures'])
def send_channel_state(self, idx, state):
if state:
self.settings['channel_states'][idx] = 1
self.sliders[idx]['on_off'].setText("On")
else:
self.settings['channel_states'][idx] = 0
self.sliders[idx]['on_off'].setText("Off")
self.send_command(command='chan', args=self.settings['channel_states'])
rospy.set_param('/config_node/channels/states',
self.settings['channel_states'])
def set_graph_state(self, value):
if value:
on_off_str = 'on'
self.graph_button.setText("Graph ON")
else:
on_off_str = 'off'
self.graph_button.setText("Graph OFF")
self.send_command(command=on_off_str, args=[])
def set_reset(self, value):
self.send_command(command='mode', args=[3])
def send_command(self, command, args, wait_for_ack=False):
if self._client_connected:
# Send commands to the command server and wait for things to be taken care of
goal = pressure_controller_ros.msg.CommandGoal(command=command,
args=args,
wait_for_ack=False)
self._client.send_goal(goal)
self._client.wait_for_result()
if not self._client.get_result():
raise ('Something went wrong and a setting was not validated')
pass
else:
pass
else:
print(command, args)
def shutdown_sliders(self):
print("")
print("Final Pressures:")
print(self.settings['pressures'])
print("")
self.set_graph_state(False)
for slider_group in self.sliders:
for widget in slider_group:
if 'on_off' in widget:
slider_group[widget].clicked.disconnect()
else:
slider_group[widget].valueChanged.disconnect()
def save_mcu_settings(self):
self.send_command(command='save', args=[])
def shutdown_plugin(self):
self._client.cancel_all_goals()
self.shutdown_sliders()
self.save_mcu_settings()
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
class MyPlugin(Plugin):
def __init__(self, context):
super(MyPlugin, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('MyPlugin')
# Process standalone plugin command-line arguments
from argparse import ArgumentParser
parser = ArgumentParser()
# Add argument(s) to the parser.
parser.add_argument("-q", "--quiet", action="store_true",
dest="quiet",
help="Put plugin in silent mode")
args, unknowns = parser.parse_known_args(context.argv())
if not args.quiet:
print 'arguments: ', args
print 'unknowns: ', unknowns
# Create QWidget
self._widget = QWidget()
# Get path to UI file which should be in the "resource" folder of this package
ui_file = os.path.join(rospkg.RosPack().get_path('pressure_controller_setup'), 'resource', 'MyPlugin.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('Set Pressures')
self.settings=self.get_settings()
self.init_sliders()
self._client = actionlib.SimpleActionClient('pressure_control', pressure_controller_ros.msg.CommandAction)
self._client_connected=self._client.wait_for_server(timeout=rospy.rostime.Duration(1))
if not self._client_connected:
print("No command server avaiable... changes will not be sent")
self.set_graph_state(True)
self.send_channel_state(0,self.settings['channel_states'][0])
# 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 init_sliders(self):
sliderbox = self._widget.findChild(QLayout,'Sliders')
firstCol = QVBoxLayout()
graph_button=QPushButton()
graph_button.setCheckable(True)
graph_button.setText("Graph Off")
graph_button.toggle()
graph_button.clicked.connect(self.set_graph_state)
self.graph_button = graph_button
reset_button=QPushButton()
reset_button.setCheckable(False)
reset_button.setText("Reset")
reset_button.clicked.connect(self.set_reset)
self.graph_button = graph_button
self.reset_button = reset_button
firstCol.addWidget(graph_button)
firstCol.addWidget(reset_button)
firstCol.addWidget(graph_button)
firstCol.setAlignment(graph_button,Qt.AlignVCenter)
zero_button=QPushButton()
zero_button.setCheckable(False)
zero_button.setText("Set All Zero")
zero_button.clicked.connect(self.set_pressure_zero)
self.zero_button = zero_button
firstCol.addWidget(zero_button)
firstCol.setAlignment(zero_button,Qt.AlignVCenter)
transition_box = QVBoxLayout()
label = QLabel()
label.setAlignment(Qt.AlignCenter)
label.setText("Transition Time")
spinbox = QDoubleSpinBox()
spinbox.setMinimum(0)
spinbox.setMaximum(10)
spinbox.setValue(self.settings['transitions'])
spinbox.setDecimals(1)
spinbox.setSingleStep(0.1)
spinbox.setSuffix(" sec")
spinbox.valueChanged.connect(self.set_transition_value)
transition_box.addWidget(label)
transition_box.addWidget(spinbox)
transition_box.setAlignment(label,Qt.AlignBottom)
transition_box.setAlignment(spinbox,Qt.AlignTop)
firstCol.addLayout(transition_box)
self.sliders = []
sliderbox.addLayout(firstCol)
all_rows_layout = QVBoxLayout()
g_idx = 0
for row in self.settings['gui_config']:
num_groups_row = len(row)
row_layout = QHBoxLayout()
for gr_idx, s_group in enumerate(row):
g_channels = s_group['channels']
g_layout = s_group['layout']
if 'horiz' in g_layout:
group_layout = QHBoxLayout()
else:
group_layout = QVBoxLayout()
control_group = {'sliders': [], 'on_off': None}
label = QLabel()
label.setText("Group. %d"%(g_idx+1))
label.setAlignment(Qt.AlignCenter)
group_layout.addWidget(label)
for c_idx, s_idx in enumerate(g_channels):
idx = s_idx*1
slider_group={'slider':None, 'number':None}
layout_cluster = QVBoxLayout()
labelfirst = QLabel()
labelfirst.setAlignment(Qt.AlignCenter)
labellast = QLabel()
labellast.setAlignment(Qt.AlignCenter)
layout = QVBoxLayout()
if 'diff' in g_layout and c_idx == 0:
sublayout=QHBoxLayout()
layout.setAlignment(Qt.AlignVCenter)
slider = QSlider(Qt.Horizontal)
slider.setMinimum(-100)
slider.setMaximum(100)
slider.setValue(0)
slider.setTickPosition(QSlider.TicksRight)
slider.setTickInterval(20)
spinbox = QDoubleSpinBox()
spinbox.setMinimum(-10)
spinbox.setMaximum(10)
spinbox.setValue(0)
spinbox.setDecimals(1)
spinbox.setSingleStep(0.1)
labellast.setText("%0.1f"%(10)) # These are flipped becasue of order
labelfirst.setText("%0.1f"%(-10))
max_label = labellast
min_label = labelfirst
else:
layout.setAlignment(Qt.AlignHCenter)
slider = QSlider(Qt.Vertical)
slider.setMinimum(self.settings['min_pressure'][idx]*10.0)
slider.setMaximum(self.settings['max_pressure'][idx]*10.0)
slider.setValue(0)
slider.setTickPosition(QSlider.TicksRight)
slider.setTickInterval(20)
spinbox = QDoubleSpinBox()
spinbox.setMinimum(self.settings['min_pressure'][idx])
spinbox.setMaximum(self.settings['max_pressure'][idx])
spinbox.setValue(0)
spinbox.setDecimals(1)
spinbox.setSingleStep(0.1)
labelfirst.setText("%0.1f"%(self.settings['max_pressure'][idx]))
labellast.setText("%0.1f"%(self.settings['min_pressure'][idx]))
max_label = labelfirst
min_label = labellast
cb_function_number = lambda value, g_idx=g_idx, s_idx=c_idx, slider=False: self.send_slider_value(g_idx,s_idx,value,slider)
cb_function_slider = lambda value, g_idx=g_idx, s_idx=c_idx, slider=True: self.send_slider_value(g_idx,s_idx,value,slider)
slider.valueChanged.connect(cb_function_slider)
spinbox.valueChanged.connect(cb_function_number)
if 'diff' in g_layout and c_idx == 0:
sublayout.addWidget(labelfirst)
sublayout.addWidget(slider)
sublayout.addWidget(labellast)
layout.addWidget(spinbox)
layout.addLayout(sublayout)
else:
layout.addWidget(labelfirst)
layout.addWidget(slider)
layout.addWidget(labellast)
layout.addWidget(spinbox)
layout.setAlignment(slider, Qt.AlignHCenter)
layout.setAlignment(spinbox, Qt.AlignHCenter)
layout_cluster.addLayout(layout)
slider_group['slider'] = slider
slider_group['number'] = spinbox
slider_group['max_label'] = max_label
slider_group['min_label'] = min_label
control_group['sliders'].append(slider_group)
group_layout.addLayout(layout_cluster)
on_button=QPushButton()
on_button.setCheckable(True)
on_button.setText("Off")
if self.settings['channel_states'][idx]:
on_button.toggle()
on_button.setText("On")
on_button.clicked.connect(lambda state, g_idx=g_idx: self.send_channel_state(g_idx,state))
group_layout.addWidget(on_button)
row_layout.addLayout(group_layout)
row_layout.addSpacing(20)
control_group['on_off'] = on_button
self.sliders.append(control_group)
g_idx+=1
all_rows_layout.addLayout(row_layout)
sliderbox.addLayout(all_rows_layout)
# for num_channels_row in self.settings['num_channels']:
# row_layout = QHBoxLayout()
# for i in range(num_channels_row):
# idx = chan_idx*1
# slider_group={'slider':None, 'number':None, 'on_off':None}
# layout_cluster = QVBoxLayout()
# layout = QVBoxLayout()
# layout.setAlignment(Qt.AlignHCenter)
# slider = QSlider(Qt.Vertical)
# slider.setMinimum(self.settings['min_pressure'][idx]*10.0)
# slider.setMaximum(self.settings['max_pressure'][idx]*10.0)
# slider.setValue(0)
# slider.setTickPosition(QSlider.TicksRight)
# slider.setTickInterval(20)
# spinbox = QDoubleSpinBox()
# spinbox.setMinimum(self.settings['min_pressure'][idx])
# spinbox.setMaximum(self.settings['max_pressure'][idx])
# spinbox.setValue(0)
# spinbox.setDecimals(1)
# spinbox.setSingleStep(0.1)
# cb_function_curr = lambda value, idx=idx, slider=False: self.send_slider_value(idx,value,slider)
# cb_function_curr2 = lambda value, idx=idx, slider=True: self.send_slider_value(idx,value,slider)
# slider.valueChanged.connect(cb_function_curr2)
# spinbox.valueChanged.connect(cb_function_curr)
# labelmax = QLabel()
# labelmax.setAlignment(Qt.AlignCenter)
# labelmax.setText("%0.1f"%(self.settings['max_pressure'][idx]))
# labelmin = QLabel()
# labelmin.setAlignment(Qt.AlignCenter)
# labelmin.setText("%0.1f"%(self.settings['min_pressure'][idx]))
# layout.addWidget(labelmax)
# layout.addWidget(slider)
# layout.addWidget(labelmin)
# layout.addWidget(spinbox)
# layout.setAlignment(slider, Qt.AlignHCenter)
# layout.setAlignment(spinbox, Qt.AlignHCenter)
# label = QLabel()
# label.setText("Chan. %d"%(chan_idx+1))
# label.setAlignment(Qt.AlignCenter)
# layout_cluster.addWidget(label)
# layout_cluster.addLayout(layout)
# slider_group['slider'] = slider
# slider_group['number'] = spinbox
# on_button=QPushButton()
# on_button.setCheckable(True)
# on_button.setText("Off")
# if self.settings['channel_states'][idx]:
# on_button.toggle()
# on_button.setText("On")
# on_button.clicked.connect(lambda state, idx=idx: self.send_channel_state(idx,state))
# slider_group['on_off'] = on_button
# layout_cluster.addWidget(on_button)
# row_layout.addLayout(layout_cluster)
# row_layout.addSpacing(20)
# self.sliders.append(slider_group)
# chan_idx+=1
# all_rows_layout.addLayout(row_layout)
# sliderbox.addLayout(all_rows_layout)
#self._widget.setLayout(sliderbox)
def get_settings(self):
settings={}
settings['channel_states'] = rospy.get_param('/config_node/channels/states',[1,1,1,1,1,1,1,1,1,1])
settings['num_channels'] = rospy.get_param('/pressure_control/num_channels',[])
settings['transitions'] = rospy.get_param('/config_node/transitions',1.0)
settings['gui_config'] = rospy.get_param('/config_node/gui_config',None)
maxp = rospy.get_param('/config_node/max_pressure',40.0)
minp = rospy.get_param('/config_node/min_pressure',-40.0)
settings['num_channels_tot'] = sum(settings['num_channels'])
if isinstance(maxp, list):
settings['max_pressure'] = maxp
elif maxp is not None:
settings['max_pressure'] = [maxp]*settings['num_channels_tot']
if isinstance(minp, list):
settings['min_pressure'] = minp
elif minp is not None:
settings['min_pressure'] = [minp]*settings['num_channels_tot']
settings['pressures'] = [0]*settings['num_channels_tot']
# If GUI Config is not located, default to rows of pressure sliders for each hardware device
if settings['gui_config'] is None:
chan_idx = 0
gui_config=[]
for num_channels in settings['num_channels']:
row=[]
for idx in range(num_channels):
row.append({'channels':[chan_idx], 'layout':'single'})
chan_idx+=1
gui_config.append(row)
settings['gui_config'] = gui_config
print(gui_config)
settings['gui_config_flat'] = []
for row in settings['gui_config']:
for group in row:
settings['gui_config_flat'].append(group)
return settings
def set_transition_value(self, value):
self.settings['transitions'] = value
rospy.set_param('/config_node/transitions',self.settings['transitions'])
def set_pressure_zero(self, value):
self.settings['pressures'] = [0]*self.settings['num_channels_tot']
self.send_command(command='set', args=[self.settings['transitions']]+self.settings['pressures'])
for slider_group in self.sliders:
for slider in slider_group['sliders']:
#slider_group['slider'].setValue(0)
slider['number'].setValue(0)
def send_slider_value(self, g_idx, s_idx, value, slider):
if slider:
press = self.sliders[g_idx]['sliders'][s_idx]['slider'].value()/10.0
self.sliders[g_idx]['sliders'][s_idx]['number'].setValue(press)
else:
press = self.sliders[g_idx]['sliders'][s_idx]['number'].value()
self.sliders[g_idx]['sliders'][s_idx]['slider'].setValue(press*10.0)
print(press)
config_set = self.settings['gui_config_flat'][g_idx]
if 'diff' in config_set['layout']:
diff_p = self.sliders[g_idx]['sliders'][0]['number'].value()
main_p = self.sliders[g_idx]['sliders'][1]['number'].value()
if 'inv' in config_set['layout']:
p1 = main_p+diff_p
p2 = main_p-diff_p
else:
p1 = main_p-diff_p
p2 = main_p+diff_p
self.settings['pressures'][config_set['channels'][0]] = p1
self.settings['pressures'][config_set['channels'][1]] = p2
maxp1=self.settings['max_pressure'][config_set['channels'][0]]
maxp2=self.settings['max_pressure'][config_set['channels'][1]]
avg_maxp = sum([maxp1, maxp2])/2.0
max_bound = avg_maxp - main_p
self.update_slider_bounds(g_idx,0,[-max_bound,max_bound])
print(g_idx, s_idx, main_p, diff_p)
else:
self.settings['pressures'][config_set['channels'][s_idx]]=press
self.send_command(command='set', args=[self.settings['transitions']]+self.settings['pressures'])
def send_channel_state(self, g_idx, state):
if state:
self.sliders[g_idx]['on_off'].setText("On")
state_num=1
else:
self.sliders[g_idx]['on_off'].setText("Off")
state_num=0
config_set = self.settings['gui_config_flat'][g_idx]
channels = config_set['channels']
for channel in channels:
self.settings['channel_states'][channel] = state_num
self.send_command(command='chan', args=self.settings['channel_states'])
rospy.set_param('/config_node/channels/states',self.settings['channel_states'])
def set_graph_state(self, value):
if value:
on_off_str='on'
self.graph_button.setText("Graph ON")
else:
on_off_str='off'
self.graph_button.setText("Graph OFF")
self.send_command(command=on_off_str, args=[])
def set_reset(self, value):
self.send_command(command='mode', args=[3])
def send_command(self, command, args, wait_for_ack=False):
if self._client_connected:
# Send commands to the command server and wait for things to be taken care of
goal = pressure_controller_ros.msg.CommandGoal(command=command, args=args, wait_for_ack = False)
self._client.send_goal(goal)
self._client.wait_for_result()
if not self._client.get_result():
raise ('Something went wrong and a setting was not validated')
pass
else:
pass
else:
print(command, args)
def update_slider_bounds(self, g_idx, s_idx, values):
self.sliders[g_idx]['sliders'][s_idx]['slider'].setMinimum(values[0]*10.0)
self.sliders[g_idx]['sliders'][s_idx]['number'].setMinimum(values[0])
self.sliders[g_idx]['sliders'][s_idx]['slider'].setMaximum(values[1]*10.0)
self.sliders[g_idx]['sliders'][s_idx]['number'].setMaximum(values[1])
self.sliders[g_idx]['sliders'][s_idx]['min_label'].setText("%0.1f"%(values[0]))
self.sliders[g_idx]['sliders'][s_idx]['max_label'].setText("%0.1f"%(values[1]))
def shutdown_sliders(self):
print("")
print("Final Pressures:")
print(self.settings['pressures'])
print("")
self.set_graph_state(False)
for slider_group in self.sliders:
for slider in slider_group['sliders']:
slider['slider'].valueChanged.disconnect()
slider['number'].valueChanged.disconnect()
on_off = slider_group['on_off']
on_off.clicked.disconnect()
def save_mcu_settings(self):
self.send_command(command='save', args=[])
def shutdown_plugin(self):
self._client.cancel_all_goals()
self.shutdown_sliders()
self.save_mcu_settings()
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
#def trigger_configuration(self):
# pass
class GaitSelectionPlugin(Plugin):
def __init__(self, context):
super(GaitSelectionPlugin, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('GaitSelectionPlugin')
# Connect to services
try:
rospy.wait_for_service('/march/gait_selection/get_version_map', 3)
rospy.wait_for_service('/march/gait_selection/get_directory_structure', 3)
rospy.wait_for_service('/march/gait_selection/set_version_map', 3)
rospy.wait_for_service('/march/gait_selection/update_default_versions', 3)
except rospy.ROSException:
rospy.logerr('Shutting down march_rqt_gait_selection, could not connect to march_gait_selection.')
rospy.signal_shutdown('Shutting down march_rqt_gait_selection, could not connect to march_gait_selection.')
sys.exit(0)
self.get_version_map = rospy.ServiceProxy('/march/gait_selection/get_version_map', Trigger)
self.get_directory_structure = rospy.ServiceProxy('/march/gait_selection/get_directory_structure', Trigger)
self.set_version_map = rospy.ServiceProxy('/march/gait_selection/set_version_map', StringTrigger)
self.update_default_versions = rospy.ServiceProxy('/march/gait_selection/update_default_versions', Trigger)
# Create QWidget
self._widget = QWidget()
# Get path to UI file which should be in the 'resource' folder of this package
ui_file = os.path.join(rospkg.RosPack().get_path('march_rqt_gait_selection'), 'resource', 'gait_selection.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('Gait Selection')
# 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)
self._widget.Gaits.findChild(QWidget, 'scrollArea').findChild(QWidget, 'content').setLayout(QVBoxLayout())
self.refresh()
self._widget.findChild(QPushButton, 'Refresh').clicked.connect(lambda: self.refresh(True))
self._widget.findChild(QPushButton, 'Apply').clicked.connect(
lambda: [
self.set_gait_selection_map(True),
self.refresh(),
])
self._widget.findChild(QPushButton, 'SaveDefault').clicked.connect(
lambda: [
self.set_gait_selection_map(),
self.update_defaults(True),
self.refresh(),
])
self.log('Welcome to the Gait Selection.', Color.Debug)
self.log('Select the versions of subgaits you want to select and press Apply.', Color.Info)
self.log('Save as default persists between launches', Color.Info)
self.log('Any warnings or errors will be displayed in this log.', Color.Warning)
self.log('--------------------------------------', Color.Info)
def log(self, msg, level):
self._widget \
.findChild(QPlainTextEdit, 'Log') \
.appendHtml('<p style="color:' + str(level.value) + '">' + msg + '</p>')
scrollbar = self._widget.findChild(QPlainTextEdit, 'Log').verticalScrollBar()
scrollbar.setValue(scrollbar.maximum())
def refresh(self, notify=False):
if notify:
self.log('Refreshing gait directory', Color.Debug)
rospy.logdebug('Refreshing ui with %s', str(self.get_directory_structure().message))
try:
gait_version_map = ast.literal_eval(self.get_version_map().message)
except ValueError:
self.log('Gait selection map is not valid'
+ str(self.get_version_map().message), Color.Error)
rospy.logerr('Gait selection map is not valid'
+ str(self.get_version_map().message))
return
try:
gait_directory_structure = ast.literal_eval(self.get_directory_structure().message)
except ValueError:
self.log('Gait directory structure is not valid '
+ str(self.get_directory_structure().message), Color.Error)
rospy.logerr('Gait directory structure is not valid '
+ str(self.get_directory_structure().message))
return
gaits = self._widget.Gaits \
.findChild(QWidget, 'scrollArea') \
.findChild(QWidget, 'content') \
.findChildren(QGroupBox, 'Gait')
for gait in gaits:
gait.deleteLater()
self.load_ui(gait_directory_structure, gait_version_map)
def load_ui(self, gait_directory_structure, gait_selection_map):
for gait_name, gait in gait_directory_structure.iteritems():
try:
selection_map = gait_selection_map[gait_name]
except KeyError:
selection_map = None
gait_group_box = self.create_gait(gait_name, gait, selection_map)
self._widget.Gaits \
.findChild(QWidget, 'scrollArea') \
.findChild(QWidget, 'content') \
.layout() \
.addWidget(gait_group_box)
def create_gait(self, name, gait, selections):
gait_group_box = QGroupBox()
gait_group_box.setObjectName('Gait')
gait_group_box.setLayout(QHBoxLayout())
gait_group_box.setTitle(name)
image = QLabel()
image.setStyleSheet(
'background: url(' + gait['image'] + ') no-repeat center center 100px 100px;')
image.setFixedSize(64, 80)
gait_group_box.layout().addWidget(image)
for subgait_name, subgait in gait['subgaits'].iteritems():
subgait_group_box = self.create_subgait(subgait_name, subgait, selections)
gait_group_box.layout().addWidget(subgait_group_box)
return gait_group_box
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 create_dropdown(self, options, selection):
try:
index = options.index(selection)
except ValueError:
rospy.loginfo('Selection %s not found in options %s.', str(selection), str(options))
index = -1
dropdown = QComboBox()
for option in options:
dropdown.addItem(option)
dropdown.setCurrentIndex(index)
return dropdown
def set_gait_selection_map(self, notify=False):
gait_selection_map = {}
gaits = self._widget.Gaits \
.findChild(QWidget, 'scrollArea') \
.findChild(QWidget, 'content') \
.findChildren(QGroupBox, 'Gait')
for gait in gaits:
gait_name = str(gait.title())
gait_selection_map[gait_name] = {}
subgaits = gait.findChildren(QGroupBox, 'Subgait')
for subgait in subgaits:
subgait_name = str(subgait.title())
version = str(subgait.findChild(QComboBox).currentText())
gait_selection_map[gait_name][subgait_name] = version
res = self.set_version_map(str(gait_selection_map))
if res.success:
if notify:
self.log('Selection applied.', Color.Debug)
rospy.loginfo(res.message)
else:
self.log(res.message, Color.Error)
self.log('Resetting to last valid selection', Color.Warning)
rospy.logwarn(res.message)
def update_defaults(self, notify=False):
res = self.update_default_versions()
if res.success:
if notify:
self.log('Default selection updated.', Color.Debug)
rospy.loginfo(res.message)
else:
self.log(res.message, Color.Error)
self.log('Resetting to last valid selection', Color.Warning)
rospy.logwarn(res.message)
class BenchmarkGui(Plugin):
set_timer_signal = pyqtSignal(int, bool)
set_benchmark_info_signal = pyqtSignal(str)
def __init__(self, context):
super(BenchmarkGui, self).__init__(context)
self.setObjectName('EUROBENCH Benchmark Control')
self._widget = QWidget()
ui_file = path.join(path.dirname(path.realpath(__file__)),
'benchmark_gui.ui')
loadUi(ui_file, self._widget)
self._widget.setObjectName('EUROBENCH Benchmark Control')
self._widget.setWindowTitle('EUROBENCH Benchmark Control')
self.benchmark_group = rospy.get_param('benchmark_group')
# UI elements
self.robot_spinbox = self._widget.findChild(QSpinBox, 'robot_spinbox')
self.run_spinbox = self._widget.findChild(QSpinBox, 'run_spinbox')
self.clock_value = self._widget.findChild(QLabel, 'clock_value')
self.benchmark_status_value = self._widget.findChild(
QLabel, 'benchmark_status_value')
self.core_status_value = self._widget.findChild(
QLabel, 'core_status_value')
self.testbed_status_value = self._widget.findChild(
QLabel, 'testbed_status_value')
self.rosbag_controller_status_value = self._widget.findChild(
QLabel, 'rosbag_controller_status_value')
self.start_button = self._widget.findChild(QPushButton, 'start_button')
self.stop_button = self._widget.findChild(QPushButton, 'stop_button')
# UI callbacks
self.start_button.clicked.connect(self.on_startbutton_click)
self.stop_button.clicked.connect(self.on_stopbutton_click)
# Disable start buttons, set their listeners
self.start_button.setEnabled(False)
self.stop_button.setEnabled(False)
# Status of required nodes
self.benchmark_core_available = False
self.rosbag_controller_available = False
self.testbed_node_available = False
self.core_status = None
# Subscribe to benchmark state
rospy.Subscriber('bmcore/state',
BenchmarkCoreState,
self.state_callback,
queue_size=1)
# Run heartbeat checks for required nodes (every second)
rospy.Timer(rospy.Duration(1), self.check_required_nodes)
# Update status labels
rospy.Timer(rospy.Duration(0.1), self.update_status_labels)
context.add_widget(self._widget)
def on_startbutton_click(self):
robot_name = self.robot_spinbox.value()
run_number = self.run_spinbox.value()
try:
start_benchmark = rospy.ServiceProxy('bmcore/start_benchmark',
StartBenchmark)
start_request = StartBenchmarkRequest()
start_request.robot_name = robot_name
start_request.run_number = run_number
start_benchmark(start_request)
except rospy.ServiceException as e:
rospy.logerr(
"bmcore/start_benchmark couldn't be called: {ex_val}".format(
ex_val=str(e)))
@staticmethod
def on_stopbutton_click():
try:
stop_benchmark = rospy.ServiceProxy('bmcore/stop_benchmark',
StopBenchmark)
stop_benchmark()
except rospy.ServiceException as e:
rospy.logerr(
"bmcore/stop_benchmark couldn't be called: {ex_val}".format(
ex_val=str(e)))
def state_callback(self, core_status_msg):
self.core_status = core_status_msg
def update_status_labels(self, _):
all_required_nodes_available = self.benchmark_core_available and self.rosbag_controller_available and self.testbed_node_available
benchmark_running = self.core_status is not None and self.core_status.status == BenchmarkCoreState.RUNNING_BENCHMARK
# ready_to_start
if all_required_nodes_available:
if not benchmark_running:
self.benchmark_status_value.setText('Ready to start')
self.run_spinbox.setEnabled(True)
self.robot_spinbox.setEnabled(True)
self.start_button.setEnabled(True)
self.stop_button.setEnabled(False)
if benchmark_running:
self.benchmark_status_value.setText('Running benchmark')
self.run_spinbox.setEnabled(False)
self.robot_spinbox.setEnabled(False)
self.start_button.setEnabled(False)
self.stop_button.setEnabled(True)
else:
self.benchmark_status_value.setText(
'Cannot start (required nodes not running)')
self.run_spinbox.setEnabled(False)
self.robot_spinbox.setEnabled(False)
self.start_button.setEnabled(False)
self.stop_button.setEnabled(False)
# Set clock timer
if benchmark_running:
seconds_passed = self.core_status.current_benchmark_seconds_passed
if seconds_passed >= 0:
minutes_passed = seconds_passed // 60
seconds_passed = seconds_passed % 60
self.clock_value.setText('%02d:%02d' %
(minutes_passed, seconds_passed))
else:
self.clock_value.setText('%02d' % seconds_passed)
else:
self.clock_value.setText('--:--')
def shutdown_plugin(self):
rospy.signal_shutdown('Shutting down')
def save_settings(self, plugin_settings, instance_settings):
# Save intrinsic configuration
pass
def restore_settings(self, plugin_settings, instance_settings):
# Restore intrinsic configuration
pass
def check_required_nodes(self, _):
# Benchmark core
try:
rospy.wait_for_service('bmcore/start_benchmark', timeout=0.5)
self.benchmark_core_available = True
except rospy.ROSException:
self.benchmark_core_available = False
# Rosbag controller
try:
rospy.wait_for_service(
'/eurobench_rosbag_controller/start_recording', timeout=0.5)
self.rosbag_controller_available = True
except rospy.ROSException:
self.rosbag_controller_available = False
# Testbed
if self.benchmark_group == 'MADROB':
set_mode_service_name = '/madrob/door/set_mode'
try:
rospy.wait_for_service(set_mode_service_name, timeout=0.5)
self.testbed_node_available = True
except rospy.ROSException:
self.testbed_node_available = False
if self.benchmark_group == 'BEAST':
some_service_name = '/beast_cart/reset_encoders'
try:
rospy.wait_for_service(some_service_name, timeout=0.5)
self.testbed_node_available = True
except rospy.ROSException:
self.testbed_node_available = False
if self.benchmark_core_available:
self.core_status_value.setText('OK')
else:
self.core_status_value.setText('Off')
if self.rosbag_controller_available:
self.rosbag_controller_status_value.setText('OK')
else:
self.rosbag_controller_status_value.setText('Off')
if self.testbed_node_available:
self.testbed_status_value.setText('OK')
else:
self.testbed_status_value.setText('Off')
class RosCanGUI(Plugin):
def __init__(self, context):
super(RosCanGUI, self).__init__(context)
self.setObjectName('RosCanSimGUI')
rp = rospkg.RosPack()
# Create QWidget
self._widget = QWidget()
# Get path to UI file which is a sibling of this file
ui_file = os.path.join(rp.get_path('ros_can_sim'), 'resource',
'RosCanSimGUI.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
self._widget.setObjectName('RosCanSimUi')
# 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()))
# setup states and missions
# enumrations taken from CanState.msg
self.states = {
CanState.AS_OFF: "OFF",
CanState.AS_READY: "READY",
CanState.AS_DRIVING: "DRIVING",
CanState.AS_EMERGENCY_BRAKE: "EMERGENCY",
CanState.AS_FINISHED: "FINISHED"
}
self.missions = {
CanState.AMI_NOT_SELECTED: "NOT_SELECTED",
CanState.AMI_ACCELERATION: "ACCELERATION",
CanState.AMI_SKIDPAD: "SKIDPAD",
CanState.AMI_AUTOCROSS: "AUTOCROSS",
CanState.AMI_TRACK_DRIVE: "TRACK_DRIVE",
CanState.AMI_AUTONOMOUS_DEMO: "AUTONOMOUS_DEMO",
CanState.AMI_ADS_INSPECTION: "ADS_INSPECTION",
CanState.AMI_ADS_EBS: "ADS_EBS",
CanState.AMI_DDT_INSPECTION_A: "DDT_INSPECTION_A",
CanState.AMI_DDT_INSPECTION_B: "DDT_INSPECTION_B",
CanState.AMI_MANUAL: "MANUAL"
}
for mission in self.missions.values():
self._widget.findChild(QComboBox,
"MissionSelectMenu").addItem(mission)
# hook up buttons to callbacks
self._widget.findChild(
QPushButton, "SetMissionButton").clicked.connect(self.setMission)
self._widget.findChild(QPushButton,
"ResetButton").clicked.connect(self.resetState)
self._widget.findChild(QPushButton,
"RequestEBS").clicked.connect(self.requestEBS)
self._widget.findChild(QPushButton,
"DriveButton").clicked.connect(self.justDrive)
# Subscribers
self.state_sub = rospy.Subscriber("/ros_can/state", CanState,
self.stateCallback)
# Publishers
self.set_mission_pub = rospy.Publisher("/ros_can/set_mission",
CanState,
queue_size=1)
# Services
self.ebs_srv = rospy.ServiceProxy("/ros_can/ebs", Trigger)
self.reset_srv = rospy.ServiceProxy("/ros_can/reset", Trigger)
# Add widget to the user interface
context.add_widget(self._widget)
def setMission(self):
"""Sends a mission request to the simulated ros_can
The mission request is of message type eufs_msgs/CanState
where only the ami_mission field is used"""
mission = self._widget.findChild(QComboBox,
"MissionSelectMenu").currentText()
rospy.logdebug("Sending mission request for " + str(mission))
# create message to be sent
mission_msg = CanState()
# find enumerated mission and set
for enum, mission_name in self.missions.items():
if mission_name == mission:
mission_msg.ami_state = enum
self.set_mission_pub.publish(mission_msg)
rospy.logdebug("Mission request sent successfully")
def resetState(self):
"""Requests ros_can to reset it's state machine"""
rospy.logdebug("Requesting ros_can_sim reset")
try:
self.reset_srv.wait_for_service(timeout=1) # 1 second
result = self.reset_srv()
rospy.logdebug("ros_can_sim reset successful")
rospy.logdebug(result)
except:
rospy.logwarn(
"Requested ros_can_sim reset but /ros_can/reset service is not available"
)
def requestEBS(self):
"""Requests ros_can to go into EMERGENCY_BRAKE state"""
rospy.logdebug("Requesting ros_can_sim reset")
try:
self.ebs_srv.wait_for_service(timeout=1) # 1 second
result = self.ebs_srv()
rospy.logdebug("ros_can_sim reset successful")
rospy.logdebug(result)
except:
rospy.logwarn(
"Requested ros_can_sim EBS but /ros_can/ebs service is not available"
)
def stateCallback(self, msg):
"""Reads the ros_can state from the message
and displays it within the GUI
Args:
msg (eufs_msgs/CanState): state of ros_can
"""
self._widget.findChild(QLabel, "StateDisplay").setText(
self.states[msg.as_state])
self._widget.findChild(QLabel, "MissionDisplay").setText(
self.missions[msg.ami_state])
def justDrive(self):
"""overrides the state machine of the car and just makes it drive"""
# create message to be sent
state_msg = CanState()
state_msg.as_state = CanState.AS_DRIVING
state_msg.ami_state = CanState.AMI_MANUAL
self.set_mission_pub.publish(state_msg)
def shutdown_plugin(self):
"""stop all publisher, subscriber and services
necessary for clean shutdown"""
self.set_mission_pub.unregister()
self.state_sub.unregister()
self.ebs_srv.close()
self.reset_srv.close()
def save_settings(self, plugin_settings, instance_settings):
# don't know how to use
# don't delete function as it breaks rqt
pass
def restore_settings(self, plugin_settings, instance_settings):
# don't know how to use
# don't delete function as it breaks rqt
pass
class SoftwareCheckPlugin(Plugin):
def __init__(self, context):
super(SoftwareCheckPlugin, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('SoftwareCheckPlugin')
# Create QWidget
self._widget = QWidget()
# Get path to UI file which should be in the 'resource' folder of this package
ui_file = os.path.join(rospkg.RosPack().get_path('march_launch'),
'resource', 'software_check.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('SoftwareCheck')
# 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)
self.log('Welcome to the Software Check.', Color.Debug)
self.log('Select the software checks you want to perform.', Color.Info)
self.check_runner = CheckRunner(self.log)
self._widget.Checks.findChild(QPushButton, 'GitBranch') \
.clicked.connect(lambda: self.run_test('GitBranch'))
self._widget.Checks.findChild(QPushButton, 'GaitFileDirectory') \
.clicked.connect(lambda: self.run_test('GaitFileDirectory'))
self._widget.Checks.findChild(QPushButton, 'URDF') \
.clicked.connect(lambda: self.run_test('URDF'))
self._widget.Checks.findChild(QPushButton, 'SlaveCount') \
.clicked.connect(lambda: self.run_test('SlaveCount'))
def log(self, msg, level):
self._widget.findChild(QPlainTextEdit,
'Log').appendHtml('<p style="color:' +
str(level.value) + '">' +
str(msg) + '</p>')
scrollbar = self._widget.findChild(QPlainTextEdit,
'Log').verticalScrollBar()
scrollbar.setValue(scrollbar.maximum())
def run_test(self, name):
result = self.check_runner.run_check_by_name(name)
self.update_test_button(name, result)
def update_test_button(self, name, result):
button = self._widget.Checks.findChild(QPushButton, name)
if result:
button.setStyleSheet('background-color: ' +
str(Color.Check_Passed.value))
else:
button.setStyleSheet('background-color: ' +
str(Color.Check_Failed.value))
class madrob_settings_gui(Plugin):
def __init__(self, context):
super(madrob_settings_gui, self).__init__(context)
self.setObjectName('MADROB Settings')
self._widget = QWidget()
ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)),
'madrob_settings_gui.ui')
loadUi(ui_file, self._widget)
self.showing_calibration = False
self._widget.setObjectName('MADROB Settings')
self._widget.setWindowTitle('MADROB Settings')
self.benchmark_type_combo = self._widget.findChild(
QComboBox, 'benchmark_type_combo')
self.door_opening_side_combo = self._widget.findChild(
QComboBox, 'door_opening_side_combo')
self.robot_approach_side_combo = self._widget.findChild(
QComboBox, 'robot_approach_side_combo')
self.calibration_button = self._widget.findChild(
QPushButton, 'show_hide_calib')
self.calibration_button.clicked.connect(self.toggle_calibration_menu)
self.calibration_menu = self._widget.findChild(QGroupBox,
'calib_groupbox')
self.calibration_menu.hide()
self.calib_loadcell_cw_button = self._widget.findChild(
QPushButton, 'calib_loadcell_cw_button')
self.calib_loadcell_cw_button.clicked.connect(
self.calibrate_loadcell_cw)
self.calib_loadcell_ccw_button = self._widget.findChild(
QPushButton, 'calib_loadcell_ccw_button')
self.calib_loadcell_ccw_button.clicked.connect(
self.calibrate_loadcell_ccw)
self.loadcell_weight_textedit = self._widget.findChild(
QPlainTextEdit, 'calib_weight_textedit')
self.calib_motor_start_button = self._widget.findChild(
QPushButton, 'calib_motor_start')
self.calib_motor_start_button.clicked.connect(
self.start_motor_calibration)
self.calib_encoder_cw_button = self._widget.findChild(
QPushButton, 'calib_encoder_cw')
self.calib_encoder_cw_button.clicked.connect(self.calibrate_encoder_cw)
self.calib_encoder_closed_button = self._widget.findChild(
QPushButton, 'calib_encoder_closed')
self.calib_encoder_closed_button.clicked.connect(
self.calibrate_encoder_closed)
self.calib_encoder_ccw_button = self._widget.findChild(
QPushButton, 'calib_encoder_ccw')
self.calib_encoder_ccw_button.clicked.connect(
self.calibrate_encoder_ccw)
self.door_opening_side_combo.addItems(['CW', 'CCW'])
self.robot_approach_side_combo.addItems(['CW', 'CCW'])
# Every second, check for MADROB's settings from the core
self.benchmark_types_set = False
rospy.Timer(rospy.Duration(1), self.check_madrob_settings)
context.add_widget(self._widget)
self.run_rospy_node()
def check_madrob_settings(self, _):
if not self.benchmark_types_set:
try:
get_madrob_settings = rospy.ServiceProxy(
'madrob/settings', MadrobSettings)
madrob_settings = get_madrob_settings()
self.benchmark_type_combo.addItems(
madrob_settings.benchmark_types)
self.benchmark_types_set = True
except rospy.ServiceException:
pass # Core not available yet
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 toggle_calibration_menu(self):
if self.showing_calibration:
self.calibration_menu.hide()
self.calibration_button.setText('Show calibration menu')
else:
self.calibration_menu.show()
self.calibration_button.setText('Hide calibration menu')
self.showing_calibration = not self.showing_calibration
def calibrate_loadcell_cw(self):
try:
weight = int(
round(float(self.loadcell_weight_textedit.toPlainText())))
except:
rospy.logerr(
'Error parsing integer for "weight": no calibration done.')
return
calibrate_handle_service_name = '/madrob/handle/calibrate'
try:
rospy.wait_for_service(calibrate_handle_service_name, timeout=1)
except rospy.ROSException:
rospy.logerr('{service_name} unavailable'.format(
service_name=calibrate_handle_service_name))
return
calibrate_handle = rospy.ServiceProxy(calibrate_handle_service_name,
CalibrateHandle)
calibrate_handle_req = CalibrateHandleRequest()
calibrate_handle_req.samples = 160
calibrate_handle_req.step = 0
calibrate_handle_req.force = -weight
calibrate_handle_res = calibrate_handle(calibrate_handle_req)
if calibrate_handle_res.success:
rospy.loginfo('Loadcell CW calibrated')
else:
rospy.logerr('Could not calibrate loadcell CW: %s' %
calibrate_handle_res.message)
def calibrate_loadcell_ccw(self):
try:
weight = int(
round(float(self.loadcell_weight_textedit.toPlainText())))
except:
rospy.logerr(
'Error parsing integer for "weight": no calibration done.')
return
calibrate_handle_service_name = '/madrob/handle/calibrate'
try:
rospy.wait_for_service(calibrate_handle_service_name, timeout=1)
except rospy.ROSException:
rospy.logerr('{service_name} unavailable'.format(
service_name=calibrate_handle_service_name))
return
calibrate_handle = rospy.ServiceProxy(calibrate_handle_service_name,
CalibrateHandle)
calibrate_handle_req = CalibrateHandleRequest()
calibrate_handle_req.samples = 160
calibrate_handle_req.step = 1
calibrate_handle_req.force = weight
calibrate_handle_res = calibrate_handle(calibrate_handle_req)
if calibrate_handle_res.success:
rospy.loginfo('Loadcell CCW calibrated')
else:
rospy.logerr('Could not calibrate loadcell CCW: %s' %
calibrate_handle_res.message)
def start_motor_calibration(self):
# TODO start motor calibration procedure
pass
def calibrate_encoder_closed(self):
self.calibrate_encoder(CalibrateDoorPositionRequest.POSITION_ZERO)
def calibrate_encoder_cw(self):
self.calibrate_encoder(CalibrateDoorPositionRequest.POSITION_CW)
def calibrate_encoder_ccw(self):
self.calibrate_encoder(CalibrateDoorPositionRequest.POSITION_CCW)
def calibrate_encoder(self, position):
calibrate_position_service_name = '/madrob/door/calibrate_position'
try:
rospy.wait_for_service(calibrate_position_service_name, timeout=1)
except rospy.ROSException:
rospy.logerr('{service_name} unavailable'.format(
service_name=calibrate_position_service_name))
return
calibrate_position = rospy.ServiceProxy(
calibrate_position_service_name, CalibrateDoorPosition)
calibrate_position_req = CalibrateDoorPositionRequest()
calibrate_position_req.position = position
calibrate_position_res = calibrate_position(calibrate_position_req)
if calibrate_position_res.success:
rospy.loginfo('Door encoder calibrated. Position: %d' % position)
else:
rospy.logerr('Could not calibrate door encoder. Position: %d' %
position)
def run_rospy_node(self):
rospy.Service('madrob/gui/benchmark_params', MadrobBenchmarkParams,
self.benchmark_params_callback)
def benchmark_params_callback(self, _):
benchmark_params_response = MadrobBenchmarkParamsResponse()
benchmark_params_response.benchmark_type = self.benchmark_type_combo.currentText(
)
benchmark_params_response.door_opening_side = self.door_opening_side_combo.currentText(
)
benchmark_params_response.robot_approach_side = self.robot_approach_side_combo.currentText(
)
return benchmark_params_response
class RqtFileDialog(Plugin):
def __init__(self, context):
super(RqtFileDialog, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('RqtFileDialog')
rp = rospkg.RosPack()
# Process standalone plugin command-line arguments
from argparse import ArgumentParser
parser = ArgumentParser()
# Add argument(s) to the parser.
parser.add_argument("-q", "--quiet", action="store_true",
dest="quiet",
help="Put plugin in silent mode")
args, unknowns = parser.parse_known_args(context.argv())
if not args.quiet:
print 'arguments: ', args
print 'unknowns: ', unknowns
# Create QWidget
self._widget = QWidget()
# Get path to UI file which is a sibling of this file
# in this example the .ui and .py file are in the same folder
ui_file = os.path.join(rp.get_path('rqt_file_dialog'), 'resource', 'file_dialog.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('RqtFileDialogUi')
# 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)
self.select_button = self._widget.findChild(QPushButton, 'select_button')
self.select_button.clicked.connect(self.handle_select)
self.current_line_edit = self._widget.findChild(QLineEdit, 'current_line_edit')
self.current_line_edit.editingFinished.connect(self.publish)
self.pub = None
def handle_select(self):
# TODO(lucasw) have a parameter define which kind of dialog to use
file_dir = self.current_line_edit.text()
new_file_dir = None
if self.mode == "file_save":
new_file_dir, tmp = QFileDialog.getSaveFileName(caption="Select a save file",
directory=os.path.dirname(file_dir))
elif self.mode == "file_open":
new_file_dir, tmp = QFileDialog.getOpenFileName(caption="Select a file",
directory=os.path.dirname(file_dir))
else: # elif mode == "dir":
new_file_dir = QFileDialog.getExistingDirectory(caption="Select a directory",
directory=os.path.dirname(file_dir))
if new_file_dir is not None and new_file_dir != "":
self.current_line_edit.setText(new_file_dir)
self.publish()
def publish(self):
if self.pub is not None:
self.pub.publish(self.current_line_edit.text())
def shutdown_plugin(self):
# TODO unregister all publishers here
pass
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value("file_dir", self.current_line_edit.text())
instance_settings.set_value("mode", self.mode)
instance_settings.set_value("select_text", self.select_button.text())
if self.pub:
instance_settings.set_value("topic", self.topic)
def restore_settings(self, plugin_settings, instance_settings):
if instance_settings.contains("file_dir"):
file_dir = instance_settings.value("file_dir")
self.current_line_edit.setText(file_dir)
self.publish()
self.mode = "file_save"
if instance_settings.contains("mode"):
self.mode = instance_settings.value("mode")
self.topic = "file_dir"
if instance_settings.contains("topic"):
self.topic = instance_settings.value("topic")
self.pub = rospy.Publisher(self.topic, String, queue_size=1, latch=True)
if instance_settings.contains("select_text"):
self.select_button.setText(instance_settings.value("select_text"))
