class PublisherTreeModel(MessageTreeModel):
_column_names = ['topic', 'type', 'rate', 'expression']
item_value_changed = Signal(int, str, str, str, object)
def __init__(self, parent=None):
super(PublisherTreeModel, self).__init__(parent)
self._column_index = {}
for column_name in self._column_names:
self._column_index[column_name] = len(self._column_index)
self.clear()
self._item_change_lock = threading.Lock()
self.itemChanged.connect(self.handle_item_changed)
def clear(self):
super(PublisherTreeModel, self).clear()
self.setHorizontalHeaderLabels(self._column_names)
def get_publisher_ids(self, index_list):
return [
item._user_data['publisher_id']
for item in self._get_toplevel_items(index_list)
]
def remove_items_with_parents(self, index_list):
for item in self._get_toplevel_items(index_list):
self.removeRow(item.row())
def handle_item_changed(self, item):
if not self._item_change_lock.acquire(False):
#qDebug('PublisherTreeModel.handle_item_changed(): could not acquire lock')
return
# lock has been acquired
topic_name = item._path
column_name = self._column_names[item.column()]
if item.isCheckable():
new_value = str(item.checkState() == Qt.Checked)
else:
new_value = item.text().strip()
#print 'PublisherTreeModel.handle_item_changed(): %s, %s, %s' % (topic_name, column_name, new_value)
self.item_value_changed.emit(item._user_data['publisher_id'],
topic_name, column_name, new_value,
item.setText)
# release lock
self._item_change_lock.release()
def remove_publisher(self, publisher_id):
for top_level_row_number in range(self.rowCount()):
item = self.item(top_level_row_number)
if item is not None and item._user_data[
'publisher_id'] == publisher_id:
self.removeRow(top_level_row_number)
return top_level_row_number
return None
def update_publisher(self, publisher_info):
top_level_row_number = self.remove_publisher(
publisher_info['publisher_id'])
self.add_publisher(publisher_info, top_level_row_number)
def add_publisher(self, publisher_info, top_level_row_number=None):
# recursively create widget items for the message's slots
parent = self
slot = publisher_info['message_instance']
slot_name = publisher_info['topic_name']
slot_type_name = publisher_info['message_instance']._type
slot_path = publisher_info['topic_name']
user_data = {'publisher_id': publisher_info['publisher_id']}
kwargs = {
'user_data': user_data,
'top_level_row_number': top_level_row_number,
'expressions': publisher_info['expressions'],
}
top_level_row = self._recursive_create_items(parent, slot, slot_name,
slot_type_name, slot_path,
**kwargs)
# fill tree widget columns of top level item
if publisher_info['enabled']:
top_level_row[self._column_index['topic']].setCheckState(
Qt.Checked)
top_level_row[self._column_index['rate']].setText(
str(publisher_info['rate']))
def _get_data_items_for_path(self, slot_name, slot_type_name, slot_path,
**kwargs):
if slot_name.startswith('/'):
return (CheckableItem(slot_name), ReadonlyItem(slot_type_name),
QStandardItem(''), ReadonlyItem(''))
expression_item = QStandardItem('')
expression_item.setToolTip(
'enter valid Python expression here, using "i" as counter and functions from math, random and time modules'
)
return (ReadonlyItem(slot_name), QStandardItem(slot_type_name),
ReadonlyItem(''), expression_item)
def _recursive_create_items(self,
parent,
slot,
slot_name,
slot_type_name,
slot_path,
expressions={},
**kwargs):
row, is_leaf_node = super(PublisherTreeModel,
self)._recursive_create_items(
parent,
slot,
slot_name,
slot_type_name,
slot_path,
expressions=expressions,
**kwargs)
if is_leaf_node:
expression_text = expressions.get(slot_path, repr(slot))
row[self._column_index['expression']].setText(expression_text)
return row
def flags(self, index):
flags = super(PublisherTreeModel, self).flags(index)
if (index.column() == self._column_index['expression']
and index.model().data(
index.model().index(
index.row(), self._column_index['type'],
index.parent()), Qt.DisplayRole) == 'bool'):
flags |= Qt.ItemIsUserCheckable
return flags
def data(self, index, role):
if (index.column() == self._column_index['expression']
and index.model().data(
index.model().index(
index.row(), self._column_index['type'],
index.parent()), Qt.DisplayRole) == 'bool'):
if role == Qt.CheckStateRole:
value = index.model().data(
index.model().index(index.row(), index.column(),
index.parent()), Qt.DisplayRole)
if value == 'True':
return Qt.Checked
if value == 'False':
return Qt.Unchecked
return Qt.PartiallyChecked
return super(PublisherTreeModel, self).data(index, role)
def setData(self, index, value, role):
if (index.column() == index.column() ==
self._column_index['expression'] and index.model().data(
index.model().index(
index.row(), self._column_index['type'],
index.parent()), Qt.DisplayRole) == 'bool'):
if role == Qt.CheckStateRole:
value = str(value == Qt.Checked)
return QStandardItemModel.setData(self, index, value,
Qt.EditRole)
return QStandardItemModel.setData(self, index, value, role)
class NodePathWorker(QObject):
finished = Signal() # class variable shared by all instances
def __init__(self,
finished_callback=None,
node_name='path_display_worker',
frame_id='world',
marker_topic='path_marker',
new_node=False,
parent=None):
super(NodePathWorker, self).__init__(parent)
if new_node:
rospy.init_node(node_name, anonymous=True)
if finished_callback is not None:
self.finished.connect(finished_callback)
#TODO([email protected]) - how to shut down?
self.is_running = True
self.frame_id = frame_id
self.marker_pub = rospy.Publisher(marker_topic,
MarkerArray,
queue_size=100)
def publish_node_path(self, x, y, z, dist=None, col_limit=1.0):
path_marker = Marker()
path_marker.header.frame_id = self.frame_id
path_marker.type = path_marker.LINE_STRIP
path_marker.id = NODE_PATH_ID
path_marker.action = path_marker.ADD
path_marker.scale.x = 0.015 #line width
path_marker.pose.orientation.w = 1.0
path_marker.pose.position.x = 0
path_marker.pose.position.y = 0
path_marker.pose.position.z = 0
alpha = 0.8
if dist is None:
path_marker.color.a = alpha
path_marker.color.r = 0.5
path_marker.color.g = 0.5
path_marker.color.b = 0.5
for i in range(0, len(x)):
path_marker.points.append(Point(x[i], y[i], z[i]))
if dist is not None:
if dist[i] <= 0:
# Black
c = std_msgs.msg.ColorRGBA(1.0, 0., 0, alpha)
elif dist[i] > col_limit:
# White
c = std_msgs.msg.ColorRGBA(0., 0.0, 1.0, alpha)
else:
# scale color
g = dist[i] / col_limit
c = std_msgs.msg.ColorRGBA(g, g, g, alpha)
path_marker.colors.append(c)
marker_array = MarkerArray()
marker_array.markers.append(path_marker)
self.marker_pub.publish(marker_array)
def publish_sub_waypoints(self, waypoints):
# Low level waypoints - plot as spheres
waypoint_marker = Marker()
waypoint_marker.header.frame_id = self.frame_id
waypoint_marker.type = waypoint_marker.SPHERE_LIST
waypoint_marker.id = SUB_WAY_ID
waypoint_marker.action = waypoint_marker.ADD
radius = 0.05
waypoint_marker.scale.x = radius #radius
waypoint_marker.scale.y = radius #radius
waypoint_marker.scale.z = radius #radius
waypoint_marker.pose.orientation.w = 1.0
waypoint_marker.pose.position.x = 0
waypoint_marker.pose.position.y = 0
waypoint_marker.pose.position.z = 0
waypoint_marker.color.r = 0.0
waypoint_marker.color.g = 1.0
waypoint_marker.color.b = 1.0
waypoint_marker.color.a = 1.0
for i in range(0, len(waypoints['x'][0, :])):
waypoint_marker.points.append(
Point(waypoints['x'][0, i], waypoints['y'][0, i],
waypoints['z'][0, i]))
marker_array = MarkerArray()
marker_array.markers.append(waypoint_marker)
self.marker_pub.publish(marker_array)
def publish_tubes(self, nodes, l_max):
# TUbes showing freespace for TACO
marker_array = MarkerArray()
for i in range(1, nodes['x'].shape[1]):
tube_marker = Marker()
tube_marker.header.frame_id = self.frame_id
tube_marker.type = tube_marker.CYLINDER
tube_marker.id = TUBE_ID + i
tube_marker.action = tube_marker.ADD
r = l_max[i - 1]
p0 = np.array([
nodes['x'][0, i - 1], nodes['y'][0, i - 1], nodes['z'][0,
i - 1]
])
p1 = np.array(
[nodes['x'][0, i], nodes['y'][0, i], nodes['z'][0, i]])
tube_marker = self.make_cylinder(tube_marker, p0, p1, r)
marker_array.markers.append(tube_marker)
self.marker_pub.publish(marker_array)
def make_cylinder(self, tube_marker, p0, p1, r):
# Cylinder size
# X and y as the diameter
tube_marker.scale.x = 2 * r
tube_marker.scale.y = 2 * r
# compute the orientation and length
# Vector between adjacent nodes
axis_vec = p1 - p0
# mid point
mid_point = p0 + axis_vec / 2
# Length of vector
length = np.linalg.norm(axis_vec)
axis_vec /= length
# Angle between vector and z axis
cos_ang = axis_vec.dot(np.array([0.0, 0.0, 1.]))
# Perpindicular vector
rot_axis = np.cross(np.array([0.0, 0.0, 1.]), axis_vec)
rot_length = np.linalg.norm(rot_axis)
rot_axis /= rot_length
# Use atan2 to get the angle
angle = np.arctan2(rot_length, cos_ang)
# Set the length
tube_marker.scale.z = length + 2 * r
# Set the orientaiton
tube_marker.pose.orientation.w = np.cos(angle / 2)
tube_marker.pose.orientation.x = rot_axis[0] * np.sin(
angle / 2
) # Without making it a normal vector it is already multiplied by sin(theta)
tube_marker.pose.orientation.y = rot_axis[1] * np.sin(angle / 2)
tube_marker.pose.orientation.z = rot_axis[2] * np.sin(angle / 2)
# Set the position
tube_marker.pose.position.x = mid_point[0]
tube_marker.pose.position.y = mid_point[1]
tube_marker.pose.position.z = mid_point[2]
tube_marker.color.a = 0.2
tube_marker.color.r = 1.0
tube_marker.color.g = 0.0
tube_marker.color.b = 0.0
return tube_marker
def hide_tubes(self, nodes):
marker_array = MarkerArray()
for i in range(1, nodes['x'].shape[1]):
tube_marker = Marker()
tube_marker.header.frame_id = self.frame_id
tube_marker.type = tube_marker.CYLINDER
tube_marker.id = TUBE_ID + i
tube_marker.action = tube_marker.DELETE
marker_array.markers.append(tube_marker)
self.marker_pub.publish(marker_array)
def stop(self):
self.is_running = False
class TrajectoryDisplayWorker(QObject):
finished = Signal() # class variable shared by all instances
def __init__(self,
finished_callback=None,
node_name='trajectory_display_worker',
frame_id='world',
marker_topic='trajectory_marker',
new_node=False,
parent=None,
qr_type="main"):
super(TrajectoryDisplayWorker, self).__init__(parent)
if new_node:
rospy.init_node(node_name, anonymous=True)
if finished_callback is not None:
self.finished.connect(finished_callback)
#TODO([email protected]) - how to shut down?
self.is_running = True
self.qr_type = qr_type
self.frame_id = frame_id
self.marker_pub = rospy.Publisher(marker_topic,
MarkerArray,
queue_size=100)
def publish_path(self, x, y, z, dist=None, col_limit=1.0):
path_marker = Marker()
path_marker.header.frame_id = self.frame_id
path_marker.type = path_marker.LINE_STRIP
if self.qr_type is "main":
path_marker.id = TRAJ_PATH_ID
elif self.qr_type is "entry":
path_marker.id = TRAJ_PATH_ID + 1
elif self.qr_type is "exit":
path_marker.id = TRAJ_PATH_ID + 2
path_marker.action = path_marker.ADD
path_marker.scale.x = 0.03 #line width
path_marker.pose.orientation.w = 1.0
path_marker.pose.position.x = 0
path_marker.pose.position.y = 0
path_marker.pose.position.z = 0
if dist is None:
if self.qr_type is "main":
path_marker.color.a = 1.0
path_marker.color.r = 1.0
path_marker.color.g = 0.5
path_marker.color.b = 0.5
elif self.qr_type is "entry":
path_marker.color.a = 1.0
path_marker.color.r = 0.5
path_marker.color.g = 1.0
path_marker.color.b = 0.5
elif self.qr_type is "exit":
path_marker.color.a = 1.0
path_marker.color.r = 1.0
path_marker.color.g = 0.0
path_marker.color.b = 0.0
for i in range(0, len(x)):
path_marker.points.append(Point(x[i], y[i], z[i]))
if dist is not None:
if dist[i] <= 0:
# Red
c = std_msgs.msg.ColorRGBA(1.0, 0, 0, 1.0)
elif dist[i] > col_limit:
# Green
c = std_msgs.msg.ColorRGBA(0.0, 1.0, 0, 1.0)
else:
# scale color
g = dist[i] / col_limit
r = 1 - g
c = std_msgs.msg.ColorRGBA(r, g, 0, 1.0)
path_marker.colors.append(c)
marker_array = MarkerArray()
marker_array.markers.append(path_marker)
self.marker_pub.publish(marker_array)
def publish_quiver(self, x, y, z, ax, ay, az):
quiver_marker_array = []
for i in range(0, len(x)):
quiver_marker = Marker()
quiver_marker.header.frame_id = self.frame_id
quiver_marker.type = quiver_marker.ARROW
quiver_marker.id = TRAJ_ACCEL_ID + i
quiver_marker.action = quiver_marker.ADD
quiver_marker.scale.x = 0.01 #line width
quiver_marker.scale.y = 0.02 #line width
quiver_marker.color.a = 1.0
quiver_marker.color.r = 1.0 if i % 10 == 0 else 0.2
quiver_marker.color.g = 0.2 if i % 10 == 0 else 1.0
quiver_marker.color.b = 0.2 if i % 10 == 0 else 0.2
scale = 0.05
quiver_marker.points.append(Point(x[i], y[i], z[i]))
quiver_marker.points.append(
Point(x[i] + scale * ax[i], y[i] + scale * ay[i],
z[i] + scale * az[i])) # + scale*9.81))
quiver_marker_array.append(quiver_marker)
for i in range(len(x), len(x) + 1000):
quiver_marker = Marker()
quiver_marker.header.frame_id = self.frame_id
quiver_marker.type = quiver_marker.ARROW
quiver_marker.id = TRAJ_ACCEL_ID + i
quiver_marker.action = quiver_marker.DELETE
quiver_marker_array.append(quiver_marker)
marker_array = MarkerArray()
marker_array.markers += quiver_marker_array
self.marker_pub.publish(marker_array)
def publish_marker_spheres(self, turning_points):
# spheres showing collision locations
marker_array = MarkerArray()
for i in range(1, turning_points['x'].size):
sphere_marker = Marker()
sphere_marker.header.frame_id = self.frame_id
sphere_marker.type = sphere_marker.SPHERE
sphere_marker.id = SPHERE_ID + i
sphere_marker.action = sphere_marker.ADD
# Scale
radius = 0.3
sphere_marker.scale.x = radius
sphere_marker.scale.y = radius
sphere_marker.scale.z = radius
sphere_marker.pose.orientation.w = 1.0
sphere_marker.pose.position.x = 0
sphere_marker.pose.position.y = 0
sphere_marker.pose.position.z = 0
# Color
sphere_marker.color.r = 1.0
sphere_marker.color.g = 0.0
sphere_marker.color.b = 0.0
sphere_marker.color.a = 0.45
# Position
sphere_marker.pose.position.x = turning_points['x'][i]
sphere_marker.pose.position.y = turning_points['y'][i]
sphere_marker.pose.position.z = turning_points['z'][i]
marker_array.markers.append(sphere_marker)
self.marker_pub.publish(marker_array)
def hide_marker_spheres(self):
marker_array = MarkerArray()
for i in range(30000):
sphere_marker = Marker()
sphere_marker.header.frame_id = self.frame_id
sphere_marker.type = sphere_marker.SPHERE
sphere_marker.id = SPHERE_ID + i
sphere_marker.action = sphere_marker.DELETE
marker_array.markers.append(sphere_marker)
self.marker_pub.publish(marker_array)
def stop(self):
self.is_running = False
class GraphThread(QObject, threading.Thread):
'''
A thread to parse file for includes
'''
graph = Signal(list, list)
'''
@ivar: graph is a signal, which emit two list for files include the current path and a list with included files.
Each entry is a tuple of the line number and path.
'''
def __init__(self, current_path, root_path):
'''
:param root_path: the open root file
:type root_path: str
:param current_path: current shown file
:type current_path: str
'''
QObject.__init__(self)
threading.Thread.__init__(self)
self.setDaemon(True)
self.current_path = current_path
self.root_path = root_path
def run(self):
'''
'''
try:
includes = self._get_includes(self.current_path)
included_from = []
incs = self._get_includes(self.root_path)
included_from = self._find_inc_file(self.current_path, incs,
self.root_path)
self.graph.emit(included_from, includes)
except Exception:
import traceback
formatted_lines = traceback.format_exc(1).splitlines()
print "Error while parse launch file for includes:\n\t%s" % traceback.format_exc(
)
try:
rospy.logwarn(
"Error while parse launch file for includes:\n\t%s",
formatted_lines[-1])
except Exception:
pass
def _get_includes(self, path):
result = []
with CHACHE_MUTEX:
if path:
if path in GRAPH_CACHE:
result = GRAPH_CACHE[path]
else:
result = LaunchConfig.included_files(path,
recursive=False,
unique=False)
GRAPH_CACHE[path] = result
return result
def _find_inc_file(self, filename, files, root_path):
result = []
for f in files:
if filename == f[1]:
result.append((f[0], root_path))
else:
inc_files = self._get_includes(f[1])
result += self._find_inc_file(filename, inc_files, f[1])
return result
class FlatOutputCommandWorker(QObject):
finished = Signal() # class variable shared by all instances
def __init__(self,
eval_callback,
dt=0.01,
finished_callback=None,
node_name='command_worker',
frame_id='command',
parent_frame_id='world',
command_topic='flat_output_setpoint',
new_node=False,
parent=None):
super(FlatOutputCommandWorker, self).__init__(parent)
if new_node:
rospy.init_node(node_name, anonymous=True)
if finished_callback is not None:
self.finished.connect(finished_callback)
#TODO([email protected]) - how to shut down?
self.is_running = True
self.publish = True
self.command = False
self.eval_callback = eval_callback
self.t = 0.
self.t_max = 0.
self.dt = dt
self.R_old = None
self.frame_id = frame_id
self.parent_frame_id = parent_frame_id
self.tf_br = tf.TransformBroadcaster()
self.command_pub = rospy.Publisher(command_topic,
FlatOutput,
queue_size=1)
print("init control")
rospy.Timer(rospy.Duration(self.dt), self.on_timer_callback)
def on_timer_callback(self, event):
if not self.publish:
return
try:
if self.command:
self.t += self.dt
if self.t > self.t_max:
self.t = self.t_max
else:
self.t = 0.
(t_max, pos_vec, qw, qx, qy, qz, yaw, vel_vec,
acc_vec) = self.eval_callback(self.t)
self.t_max = t_max
self.tf_br.sendTransform(
(pos_vec[0], pos_vec[1], pos_vec[2]), (qx, qy, qz, qw),
rospy.Time.now(), self.frame_id, self.parent_frame_id)
msg = FlatOutput()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = self.frame_id
msg.position.x = pos_vec[0]
msg.position.y = pos_vec[1]
msg.position.z = pos_vec[2]
msg.velocity.x = vel_vec[0]
msg.velocity.y = vel_vec[1]
msg.velocity.z = vel_vec[2]
msg.acceleration.x = acc_vec[0]
msg.acceleration.y = acc_vec[1]
msg.acceleration.z = acc_vec[2]
msg.yaw = yaw
self.command_pub.publish(msg)
#except Exception as e:
#print("Error in AnimateWorker")
#print(e)
except:
# print("Unknown error in AnimateWorker")
return
def start_command(self):
print("Ros helper start command")
self.command = True
def stop_command(self):
self.command = False
def start_publish(self):
self.t = 0
self.publish = True
def stop_publish(self):
self.publish = False
class DoubleEditor(EditorWidget):
_update_signal = Signal(float)
def __init__(self, updater, config):
super(DoubleEditor, self).__init__(updater, config)
loadUi(ui_num, self)
# Handle unbounded doubles nicely
if config['min'] != -float('inf'):
self._min = float(config['min'])
self._min_val_label.setText(str(self._min))
else:
self._min = -1e10000
self._min_val_label.setText('-inf')
if config['max'] != float('inf'):
self._max = float(config['max'])
self._max_val_label.setText(str(self._max))
else:
self._max = 1e10000
self._max_val_label.setText('inf')
if config['min'] != -float('inf') and config['max'] != float('inf'):
self._func = lambda x: x
self._ifunc = self._func
else:
self._func = lambda x: math.atan(x)
self._ifunc = lambda x: math.tan(x)
# If we have no range, disable the slider
self.scale = (self._func(self._max) - self._func(self._min))
if self.scale <= 0:
self.scale = 0
self.setDisabled(True)
else:
self.scale = 100 / self.scale
# Set ranges
self._slider_horizontal.setRange(self._get_value_slider(self._min),
self._get_value_slider(self._max))
validator = QDoubleValidator(self._min, self._max, 8, self)
validator.setLocale(QLocale(QLocale.C))
self._paramval_lineEdit.setValidator(validator)
# Initialize to defaults
self._paramval_lineEdit.setText(str(config['default']))
self._slider_horizontal.setValue(
self._get_value_slider(config['default']))
# Make slider update text (locally)
self._slider_horizontal.sliderMoved.connect(self._slider_moved)
# Make keyboard input change slider position and update param server
self._paramval_lineEdit.editingFinished.connect(self._text_changed)
# Make slider update param server
# Turning off tracking means this isn't called during a drag
self._slider_horizontal.setTracking(False)
self._slider_horizontal.valueChanged.connect(self._slider_changed)
# Make the param server update selection
self._update_signal.connect(self._update_gui)
# Add special menu items
self.cmenu.addAction(self.tr('Set to Maximum')).triggered.connect(self._set_to_max)
self.cmenu.addAction(self.tr('Set to Minimum')).triggered.connect(self._set_to_min)
self.cmenu.addAction(self.tr('Set to NaN')).triggered.connect(self._set_to_nan)
def _slider_moved(self):
# This is a "local" edit - only change the text
self._paramval_lineEdit.setText('{0:f}'.format(Decimal(str(
self._get_value_textfield()))))
def _text_changed(self):
# This is a final change - update param server
# No need to update slider... update_value() will
self._update_paramserver(float(self._paramval_lineEdit.text()))
def _slider_changed(self):
# This is a final change - update param server
# No need to update text... update_value() will
self._update_paramserver(self._get_value_textfield())
def _get_value_textfield(self):
'''@return: Current value in text field.'''
return self._ifunc(self._slider_horizontal.sliderPosition() /
self.scale) if self.scale else 0
def _get_value_slider(self, value):
'''
@rtype: double
'''
return int(round((self._func(value)) * self.scale))
def update_value(self, value):
super(DoubleEditor, self).update_value(value)
self._update_signal.emit(float(value))
def _update_gui(self, value):
# Block all signals so we don't loop
self._slider_horizontal.blockSignals(True)
# Update the slider value if not NaN
if not math.isnan(value):
self._slider_horizontal.setValue(self._get_value_slider(value))
elif not math.isnan(self.param_default):
self._slider_horizontal.setValue(self._get_value_slider(self.param_default))
# Make the text match
self._paramval_lineEdit.setText('{0:f}'.format(Decimal(str(value))))
self._slider_horizontal.blockSignals(False)
def _set_to_max(self):
self._update_paramserver(self._max)
def _set_to_min(self):
self._update_paramserver(self._min)
def _set_to_nan(self):
self._update_paramserver(float('NaN'))
class CapabilityTable(QTableWidget):
'''
The table shows all detected capabilities of robots in tabular view. The
columns represents the robot and rows the capabilities. The cell of available
capability contains a L{CapabilityControlWidget} to show the state and manage
the capability.
'''
start_nodes_signal = Signal(str, str, list)
'''@ivar: the signal is emitted to start on host(described by masteruri) the nodes described in the list, Parameter(masteruri, config, nodes).'''
stop_nodes_signal = Signal(str, list)
'''@ivar: the signal is emitted to stop on masteruri the nodes described in the list.'''
description_requested_signal = Signal(str, str)
'''@ivar: the signal is emitted by click on a header to show a description.'''
def __init__(self, parent=None):
QTableWidget.__init__(self, parent)
self._robotHeader = CapabilityHeader(Qt.Horizontal, self)
self._robotHeader.description_requested_signal.connect(
self._show_description)
self.setHorizontalHeader(self._robotHeader)
self._capabilityHeader = CapabilityHeader(Qt.Vertical, self)
self._capabilityHeader.description_requested_signal.connect(
self._show_description)
self.setVerticalHeader(self._capabilityHeader)
def updateCapabilities(self, masteruri, cfg_name, description):
'''
Updates the capabilities view.
:param str masteruri: the ROS master URI of updated ROS master.
:param str cfg_name: The name of the node provided the capabilities description.
:param description: The capabilities description object.
:type description: fkie_node_manager_daemon.launch_description.LaunchDescription
'''
# if it is a new masteruri add a new column
robot_name = description.robot_name
robot_index = self._robotHeader.index(masteruri)
# append a new robot
descr_utf8 = utf8(description.robot_descr.replace("\\n ", "\n"))
if robot_index == -1:
robot_index = self._robotHeader.insertSortedItem(
masteruri, robot_name)
self.insertColumn(robot_index)
self._robotHeader.setDescription(robot_index, cfg_name, masteruri,
robot_name,
description.robot_type,
descr_utf8,
description.robot_images)
item = QTableWidgetItem()
item.setSizeHint(QSize(96, 96))
self.setHorizontalHeaderItem(robot_index, item)
if robot_name:
self.horizontalHeaderItem(robot_index).setText(robot_name)
else:
# update
self._robotHeader.setDescription(robot_index, cfg_name, masteruri,
robot_name,
description.robot_type,
descr_utf8,
description.robot_images)
# set the capabilities
for c in description.capabilities:
cname = utf8(c.name)
cdescription = utf8(c.description.replace("\\n ", "\n"))
cap_index = self._capabilityHeader.index(cname)
if cap_index == -1 or self.cellWidget(cap_index,
robot_index) is None:
if cap_index == -1:
# append a new capability
cap_index = self._capabilityHeader.insertSortedItem(
cname, cname)
self.insertRow(cap_index)
self.setRowHeight(cap_index, 96)
self._capabilityHeader.setDescription(
cap_index, cfg_name, cname, cname, c.type,
cdescription, c.images)
item = QTableWidgetItem()
item.setSizeHint(QSize(96, 96))
self.setVerticalHeaderItem(cap_index, item)
self.verticalHeaderItem(cap_index).setText(cname)
else:
self._capabilityHeader.setDescription(
cap_index, cfg_name, cname, cname, c.type,
cdescription, c.images)
# add the capability control widget
controlWidget = CapabilityControlWidget(
masteruri, cfg_name, c.namespace, c.nodes)
controlWidget.start_nodes_signal.connect(self._start_nodes)
controlWidget.stop_nodes_signal.connect(self._stop_nodes)
self.setCellWidget(cap_index, robot_index, controlWidget)
self._capabilityHeader.controlWidget.insert(
cap_index, controlWidget)
else:
self._capabilityHeader.update_description(
cap_index, cfg_name, cname, cname, c.type, cdescription,
c.images)
try:
self.cellWidget(cap_index, robot_index).updateNodes(
cfg_name, c.namespace, c.nodes)
except Exception:
import traceback
print(traceback.format_exc())
def removeConfig(self, cfg):
'''
:param str cfg: The name of the node provided the capabilities description.
'''
removed_from_robots = self._robotHeader.removeCfg(cfg)
# for r in removed_from_robots:
# if not self._robotHeader.getConfigs(r):
# #remove the column with robot
# pass
removed_from_caps = self._capabilityHeader.removeCfg(cfg)
# remove control widget with given configuration
for r in reversed(removed_from_robots):
for c in removed_from_caps:
controlWidget = self.cellWidget(c, r)
if isinstance(controlWidget, CapabilityControlWidget):
controlWidget.removeCfg(cfg)
if not controlWidget.hasConfigs():
self.removeCellWidget(c, r)
# remove empty columns
for r in removed_from_robots:
is_empty = True
for c in reversed(range(self.rowCount())):
controlWidget = self.cellWidget(c, r)
if isinstance(controlWidget, CapabilityControlWidget):
is_empty = False
break
if is_empty:
self.removeColumn(r)
self._robotHeader.removeDescription(r)
# remove empty rows
for c in reversed(removed_from_caps):
is_empty = True
for r in reversed(range(self.columnCount())):
controlWidget = self.cellWidget(c, r)
if isinstance(controlWidget, CapabilityControlWidget):
is_empty = False
break
if is_empty:
self.removeRow(c)
self._capabilityHeader.removeDescription(c)
def updateState(self, masteruri, master_info):
'''
Updates the run state of the capability.
:param str masteruri: The ROS master, which sends the master_info
:param master_info: The state of the ROS master
:type master_info: U{fkie_master_discovery.MasterInfo<http://docs.ros.org/api/fkie_master_discovery/html/modules.html#module-fkie_master_discovery.master_info>}
'''
if master_info is None or masteruri is None:
return
robot_index = self._robotHeader.index(masteruri)
if robot_index != -1:
for c in range(self.rowCount()):
controlWidget = self.cellWidget(c, robot_index)
if controlWidget is not None:
running_nodes = []
stopped_nodes = []
error_nodes = []
for n in controlWidget.nodes():
node = master_info.getNode(n)
if node is not None:
# while a synchronization there are node from other hosts in the master_info -> filter these nodes
if node.uri is not None and masteruri == node.masteruri:
if node.pid is not None:
running_nodes.append(n)
else:
error_nodes.append(n)
else:
stopped_nodes.append(n)
controlWidget.setNodeState(running_nodes, stopped_nodes,
error_nodes)
def _start_nodes(self, masteruri, cfg, nodes):
self.start_nodes_signal.emit(masteruri, cfg, nodes)
def _stop_nodes(self, masteruri, nodes):
self.stop_nodes_signal.emit(masteruri, nodes)
def _show_description(self, name, description):
self.description_requested_signal.emit(name, description)
class GroupWidget(QWidget):
'''
(Isaac's guess as of 12/13/2012)
This class bonds multiple Editor instances that are associated with
a single node as a group.
'''
# public signal
sig_node_disabled_selected = Signal(str)
def __init__(self, updater, config, nodename):
'''
:param config:
:type config: Dictionary? defined in dynamic_reconfigure.client.Client
:type nodename: str
'''
#TODO figure out what data type 'config' is. It is afterall returned
# from dynamic_reconfigure.client.get_parameter_descriptions()
# ros.org/doc/api/dynamic_reconfigure/html/dynamic_reconfigure.client-pysrc.html#Client
super(GroupWidget, self).__init__()
self.state = config['state']
self.name = config['name']
self._toplevel_treenode_name = nodename
# TODO: .ui file needs to be back into usage in later phase.
# ui_file = os.path.join(rp.get_path('rqt_reconfigure'),
# 'resource', 'singlenode_parameditor.ui')
# loadUi(ui_file, self)
verticalLayout = QVBoxLayout(self)
verticalLayout.setContentsMargins(QMargins(0, 0, 0, 0))
_widget_nodeheader = QWidget()
_h_layout_nodeheader = QHBoxLayout(_widget_nodeheader)
_h_layout_nodeheader.setContentsMargins(QMargins(0, 0, 0, 0))
self.nodename_qlabel = QLabel(self)
font = QFont('Trebuchet MS, Bold')
font.setUnderline(True)
font.setBold(True)
# Button to close a node.
_icon_disable_node = QIcon.fromTheme('window-close')
_bt_disable_node = QPushButton(_icon_disable_node, '', self)
_bt_disable_node.setToolTip('Hide this node')
_bt_disable_node_size = QSize(36, 24)
_bt_disable_node.setFixedSize(_bt_disable_node_size)
_bt_disable_node.pressed.connect(self._node_disable_bt_clicked)
_h_layout_nodeheader.addWidget(self.nodename_qlabel)
_h_layout_nodeheader.addWidget(_bt_disable_node)
self.nodename_qlabel.setAlignment(Qt.AlignCenter)
font.setPointSize(10)
self.nodename_qlabel.setFont(font)
grid_widget = QWidget(self)
self.grid = QFormLayout(grid_widget)
verticalLayout.addWidget(_widget_nodeheader)
verticalLayout.addWidget(grid_widget, 1)
# Again, these UI operation above needs to happen in .ui file.
self.tab_bar = None # Every group can have one tab bar
self.tab_bar_shown = False
self.updater = updater
self.editor_widgets = []
self._param_names = []
self._create_node_widgets(config)
rospy.logdebug('Groups node name={}'.format(nodename))
self.nodename_qlabel.setText(nodename)
# Labels should not stretch
#self.grid.setColumnStretch(1, 1)
#self.setLayout(self.grid)
def collect_paramnames(self, config):
pass
def _create_node_widgets(self, config):
'''
:type config: Dict?
'''
i_debug = 0
for param in config['parameters']:
begin = time.time() * 1000
editor_type = '(none)'
if param['edit_method']:
widget = EnumEditor(self.updater, param)
elif param['type'] in EDITOR_TYPES:
rospy.logdebug('GroupWidget i_debug=%d param type =%s',
i_debug, param['type'])
editor_type = EDITOR_TYPES[param['type']]
widget = eval(editor_type)(self.updater, param)
self.editor_widgets.append(widget)
self._param_names.append(param['name'])
rospy.logdebug('groups._create_node_widgets num editors=%d',
i_debug)
end = time.time() * 1000
time_elap = end - begin
rospy.logdebug('ParamG editor={} loop=#{} Time={}msec'.format(
editor_type, i_debug, time_elap))
i_debug += 1
for name, group in config['groups'].items():
if group['type'] == 'tab':
widget = TabGroup(self, self.updater, group)
elif group['type'] in _GROUP_TYPES.keys():
widget = eval(_GROUP_TYPES[group['type']])(self.updater, group)
self.editor_widgets.append(widget)
rospy.logdebug(
'groups._create_node_widgets ' +
#'num groups=%d' +
'name=%s',
name)
for i, ed in enumerate(self.editor_widgets):
ed.display(self.grid)
rospy.logdebug('GroupWdgt._create_node_widgets len(editor_widgets)=%d',
len(self.editor_widgets))
def display(self, grid, row):
# groups span across all columns
grid.addWidget(self, row, 0, 1, -1)
def update_group(self, config):
self.state = config['state']
# TODO: should use config.keys but this method doesnt exist
names = [name for name in config.items()]
for widget in self.editor_widgets:
if isinstance(widget, EditorWidget):
if widget.name in names:
widget.update_value(config[widget.name])
elif isinstance(widget, GroupWidget):
cfg = find_cfg(config, widget.name)
widget.update_group(cfg)
def close(self):
for w in self.editor_widgets:
w.close()
def get_treenode_names(self):
'''
:rtype: str[]
'''
return self._param_names
def _node_disable_bt_clicked(self):
rospy.logdebug('param_gs _node_disable_bt_clicked')
self.sig_node_disabled_selected.emit(self._toplevel_treenode_name)
class DataPlot(QWidget):
"""A widget for displaying a plot of data
The DataPlot widget displays a plot, on one of several plotting backends,
depending on which backend(s) are available at runtime. It currently
supports PyQtGraph, MatPlot and QwtPlot backends.
The DataPlot widget manages the plot backend internally, and can save
and restore the internal state using `save_settings` and `restore_settings`
functions.
Currently, the user MUST call `restore_settings` before using the widget,
to cause the creation of the enclosed plotting widget.
"""
# plot types in order of priority
plot_types = [
{
'title': 'PyQtGraph',
'widget_class': PyQtGraphDataPlot,
'description':
'Based on PyQtGraph\n- installer: http://luke.campagnola.me/code/pyqtgraph\n',
'enabled': 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\n',
'enabled':
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\n',
'enabled':
QwtDataPlot is not None,
},
]
# pre-defined colors:
RED = (255, 0, 0)
GREEN = (0, 255, 0)
BLUE = (0, 0, 255)
SCALE_ALL = 1
SCALE_VISIBLE = 2
SCALE_EXTEND = 4
_colors = [
Qt.blue, Qt.red, Qt.cyan, Qt.magenta, Qt.green, Qt.darkYellow,
Qt.black, Qt.darkCyan, Qt.darkRed, Qt.gray
]
limits_changed = Signal()
_redraw = Signal()
_add_curve = Signal(str, str, 'QColor', bool, bool)
def __init__(self, parent=None):
"""Create a new, empty DataPlot
This will raise a RuntimeError if none of the supported plotting
backends can be found
"""
super(DataPlot, self).__init__(parent)
self.x_width = 5.0
self._plot_index = 0
self._color_index = 0
self._markers_on = False
self._autoscroll = True
self._autoscale_x = False
self._autoscale_y = DataPlot.SCALE_ALL
# the backend widget that we're trying to hide/abstract
self._data_plot_widget = None
self._curves = {}
self._vline = None
self._redraw.connect(self._do_redraw)
self._layout = QHBoxLayout()
self.setLayout(self._layout)
enabled_plot_types = [pt for pt in self.plot_types if pt['enabled']]
if not enabled_plot_types:
if qVersion().startswith('4.'):
version_info = '1.1.0'
else:
# minimum matplotlib version for Qt 5
version_info = '1.4.0'
if QT_BINDING == 'pyside':
version_info += ' and PySide %s' % \
('> 1.1.0' if qVersion().startswith('4.') else '>= 2.0.0')
raise RuntimeError(
'No usable plot type found. Install at least one of: PyQtGraph, MatPlotLib '
'(at least %s) or Python-Qwt5.' % version_info)
self._switch_data_plot_widget(self._plot_index)
self.show()
def set_x_width(self, width):
self.x_width = width
def _switch_data_plot_widget(self, plot_index, markers_on=False):
"""Internal method for activating a plotting backend by index"""
# check if selected plot type is available
if not self.plot_types[plot_index]['enabled']:
# find other available plot type
for index, plot_type in enumerate(self.plot_types):
if plot_type['enabled']:
plot_index = index
break
self._plot_index = plot_index
self._markers_on = markers_on
selected_plot = self.plot_types[plot_index]
print("Selected plot: {}".format(selected_plot['title']))
if self._data_plot_widget:
x_limits = self.get_xlim()
y_limits = self.get_ylim()
self._layout.removeWidget(self._data_plot_widget)
self._data_plot_widget.close()
self._data_plot_widget = None
else:
x_limits = [0.0, 10.0]
y_limits = [-0.001, 0.001]
self._data_plot_widget = selected_plot['widget_class'](self)
self._data_plot_widget.limits_changed.connect(self.limits_changed)
self._add_curve.connect(self._data_plot_widget.add_curve)
self._layout.addWidget(self._data_plot_widget)
# restore old data
for curve_id in self._curves:
curve = self._curves[curve_id]
self._data_plot_widget.add_curve(curve_id, curve['name'],
curve['color'], markers_on)
if self._vline:
self.vline(*self._vline)
self.set_xlim(x_limits)
self.set_ylim(y_limits)
self.redraw()
def _switch_plot_markers(self, markers_on):
self._markers_on = markers_on
self._data_plot_widget._color_index = 0
for curve_id in self._curves:
self._data_plot_widget.remove_curve(curve_id)
curve = self._curves[curve_id]
self._data_plot_widget.add_curve(curve_id, curve['name'],
curve['color'], markers_on,
curve['dashed'])
self.redraw()
# interface out to the managing GUI component: get title, save, restore,
# etc
def getTitle(self):
"""get the title of the current plotting backend"""
return self.plot_types[self._plot_index]['title']
def save_settings(self, plugin_settings, instance_settings):
"""Save the settings associated with this widget
Currently, this is just the plot type, but may include more useful
data in the future"""
instance_settings.set_value('plot_type', self._plot_index)
xlim = self.get_xlim()
ylim = self.get_ylim()
# convert limits to normal arrays of floats; some backends return numpy
# arrays
xlim = [float(x) for x in xlim]
ylim = [float(y) for y in ylim]
instance_settings.set_value('x_limits', pack(xlim))
instance_settings.set_value('y_limits', pack(ylim))
def restore_settings(self, plugin_settings, instance_settings):
"""Restore the settings for this widget
Currently, this just restores the plot type."""
self._switch_data_plot_widget(
int(instance_settings.value('plot_type', 0)))
xlim = unpack(instance_settings.value('x_limits', []))
ylim = unpack(instance_settings.value('y_limits', []))
if xlim:
# convert limits to an array of floats; they're often lists of
# strings
try:
xlim = [float(x) for x in xlim]
self.set_xlim(xlim)
except:
qWarning("Failed to restore X limits")
if ylim:
try:
ylim = [float(y) for y in ylim]
self.set_ylim(ylim)
except:
qWarning("Failed to restore Y limits")
def doSettingsDialog(self):
"""Present the user with a dialog for choosing the plot backend
This displays a SimpleSettingsDialog asking the user to choose a
plot type, gets the result, and updates the plot type as necessary
This method is blocking"""
marker_settings = [{
'title':
'Show Plot Markers',
'description':
'Warning: Displaying markers in rqt_plot may cause\n \t high cpu load, '
'especially using PyQtGraph\n',
'enabled':
True,
}]
if self._markers_on:
selected_checkboxes = [0]
else:
selected_checkboxes = []
dialog = SimpleSettingsDialog(title='Plot Options')
dialog.add_exclusive_option_group(title='Plot Type',
options=self.plot_types,
selected_index=self._plot_index)
dialog.add_checkbox_group(title='Plot Markers',
options=marker_settings,
selected_indexes=selected_checkboxes)
[plot_type, checkboxes] = dialog.get_settings()
if plot_type is not None and \
plot_type['selected_index'] is not None and \
self._plot_index != plot_type['selected_index']:
self._switch_data_plot_widget(plot_type['selected_index'], 0
in checkboxes['selected_indexes'])
else:
if checkboxes is not None and self._markers_on != (
0 in checkboxes['selected_indexes']):
self._switch_plot_markers(0 in checkboxes['selected_indexes'])
# interface out to the managing DATA component: load data, update data,
# etc
def autoscroll(self, enabled=True):
"""Enable or disable autoscrolling of the plot"""
self._autoscroll = enabled
def redraw(self):
self._redraw.emit()
def _do_redraw(self):
"""Redraw the underlying plot
This causes the underlying plot to be redrawn. This is usually used
after adding or updating the plot data"""
if self._data_plot_widget:
self._merged_autoscale()
for curve_id in self._curves:
curve = self._curves[curve_id]
try:
self._data_plot_widget.set_values(curve_id, curve['x'],
curve['y'])
except KeyError:
# skip curve which has been removed in the mean time
pass
self._data_plot_widget.redraw()
def _get_curve(self, curve_id):
if curve_id in self._curves:
return self._curves[curve_id]
else:
raise DataPlotException("No curve named %s in this DataPlot" %
(curve_id))
def add_curve(self,
curve_id,
curve_name,
data_x,
data_y,
curve_color=None,
dashed=False):
"""Add a new, named curve to this plot
Add a curve named `curve_name` to the plot, with initial data series
`data_x` and `data_y`.
Future references to this curve should use the provided `curve_id`
Note that the plot is not redraw automatically; call `redraw()` to make
any changes visible to the user.
"""
if curve_color is None:
curve_color = QColor(self._colors[self._color_index %
len(self._colors)])
self._color_index += 1
self._curves[curve_id] = {
'x': numpy.array(data_x),
'y': numpy.array(data_y),
'name': curve_name,
'color': curve_color,
'dashed': dashed
}
if self._data_plot_widget:
self._add_curve.emit(curve_id, curve_name, curve_color,
self._markers_on, dashed)
def remove_curve(self, curve_id):
"""Remove the specified curve from this plot"""
# TODO: do on UI thread with signals
if curve_id in self._curves:
del self._curves[curve_id]
if self._data_plot_widget:
self._data_plot_widget.remove_curve(curve_id)
def update_values(self, curve_id, values_x, values_y, sort_data=True):
"""Append new data to an existing curve
`values_x` and `values_y` will be appended to the existing data for
`curve_id`
Note that the plot is not redraw automatically; call `redraw()` to make
any changes visible to the user.
If `sort_data` is set to False, values won't be sorted by `values_x`
order.
"""
curve = self._get_curve(curve_id)
curve['x'] = numpy.append(curve['x'], values_x)
curve['y'] = numpy.append(curve['y'], values_y)
if sort_data:
# sort resulting data, so we can slice it later
sort_order = curve['x'].argsort()
curve['x'] = curve['x'][sort_order]
curve['y'] = curve['y'][sort_order]
def clear_values(self, curve_id=None):
"""Clear the values for the specified curve, or all curves
This will erase the data series associaed with `curve_id`, or all
curves if `curve_id` is not present or is None
Note that the plot is not redraw automatically; call `redraw()` to make
any changes visible to the user.
"""
# clear internal curve representation
if curve_id:
curve = self._get_curve(curve_id)
curve['x'] = numpy.array([])
curve['y'] = numpy.array([])
else:
for curve_id in self._curves:
self._curves[curve_id]['x'] = numpy.array([])
self._curves[curve_id]['y'] = numpy.array([])
def vline(self, x, color=RED):
"""Draw a vertical line on the plot
Draw a line a position X, with the given color
@param x: position of the vertical line to draw
@param color: optional parameter specifying the color, as tuple of
RGB values from 0 to 255
"""
self._vline = (x, color)
if self._data_plot_widget:
self._data_plot_widget.vline(x, color)
# autoscaling methods
def set_autoscale(self, x=None, y=None):
"""Change autoscaling of plot axes
if a parameter is not passed, the autoscaling setting for that axis is
not changed
@param x: enable or disable autoscaling for X
@param y: set autoscaling mode for Y
"""
if x is not None:
self._autoscale_x = x
if y is not None:
self._autoscale_y = y
# autoscaling: adjusting the plot bounds fit the data
# autoscrollig: move the plot X window to show the most recent data
#
# what order do we do these adjustments in?
# * assuming the various stages are enabled:
# * autoscale X to bring all data into view
# * else, autoscale X to determine which data we're looking at
# * autoscale Y to fit the data we're viewing
#
# * autoscaling of Y might have several modes:
# * scale Y to fit the entire dataset
# * scale Y to fit the current view
# * increase the Y scale to fit the current view
#
# TODO: incrmenetal autoscaling: only update the autoscaling bounds
# when new data is added
def _merged_autoscale(self):
x_limit = [numpy.inf, -numpy.inf]
if self._autoscale_x:
for curve_id in self._curves:
curve = self._curves[curve_id]
if len(curve['x']) > 0:
x_limit[0] = min(x_limit[0], curve['x'].min())
x_limit[1] = max(x_limit[1], curve['x'].max())
elif self._autoscroll:
# get current width of plot
x_limit = self.get_xlim()
# x_width = x_limit[1] - x_limit[0]
x_width = self.x_width
# reset the upper x_limit so that we ignore the previous position
x_limit[1] = -numpy.inf
# get largest X value
for curve_id in self._curves:
curve = self._curves[curve_id]
if len(curve['x']) > 0:
x_limit[1] = max(x_limit[1], curve['x'].max())
# set lower limit based on width
x_limit[0] = x_limit[1] - x_width
else:
# don't modify limit, or get it from plot
x_limit = self.get_xlim()
# set sane limits if our limits are infinite
if numpy.isinf(x_limit[0]):
x_limit[0] = 0.0
if numpy.isinf(x_limit[1]):
x_limit[1] = 1.0
y_limit = [numpy.inf, -numpy.inf]
if self._autoscale_y:
# if we're extending the y limits, initialize them with the
# current limits
if self._autoscale_y & DataPlot.SCALE_EXTEND:
y_limit = self.get_ylim()
for curve_id in self._curves:
curve = self._curves[curve_id]
start_index = 0
end_index = len(curve['x'])
# if we're scaling based on the visible window, find the
# start and end indicies of our window
if self._autoscale_y & DataPlot.SCALE_VISIBLE:
# indexof x_limit[0] in curves['x']
start_index = curve['x'].searchsorted(x_limit[0])
# indexof x_limit[1] in curves['x']
end_index = curve['x'].searchsorted(x_limit[1])
# region here is cheap because it is a numpy view and not a
# copy of the underlying data
region = curve['y'][start_index:end_index]
if len(region) > 0:
y_limit[0] = min(y_limit[0], region.min())
y_limit[1] = max(y_limit[1], region.max())
# TODO: compute padding around new min and max values
# ONLY consider data for new values; not
# existing limits, or we'll add padding on top of old
# padding in SCALE_EXTEND mode
#
# pad the min/max
# TODO: invert this padding in get_ylim
# ymin = limits[0]
# ymax = limits[1]
# delta = ymax - ymin if ymax != ymin else 0.1
# ymin -= .05 * delta
# ymax += .05 * delta
else:
y_limit = self.get_ylim()
# set sane limits if our limits are infinite
if numpy.isinf(y_limit[0]):
y_limit[0] = 0.0
if numpy.isinf(y_limit[1]):
y_limit[1] = 1.0
self.set_xlim(x_limit)
self.set_ylim(y_limit)
def get_xlim(self):
"""get X limits"""
if self._data_plot_widget:
return self._data_plot_widget.get_xlim()
else:
qWarning("No plot widget; returning default X limits")
return [0.0, 1.0]
def set_xlim(self, limits):
"""set X limits"""
if self._data_plot_widget:
self._data_plot_widget.set_xlim(limits)
else:
qWarning("No plot widget; can't set X limits")
def get_ylim(self):
"""get Y limits"""
if self._data_plot_widget:
return self._data_plot_widget.get_ylim()
else:
qWarning("No plot widget; returning default Y limits")
return [0.0, 10.0]
def set_ylim(self, limits):
"""set Y limits"""
if self._data_plot_widget:
self._data_plot_widget.set_ylim(limits)
else:
qWarning("No plot widget; can't set Y limits")
class JointStatePublisherGui(QWidget):
sliderUpdateTrigger = Signal()
def __init__(self, title, jsp, num_rows=0):
super(JointStatePublisherGui, self).__init__()
self.jsp = jsp
self.joint_map = {}
self.vlayout = QVBoxLayout(self)
self.scrollable = QWidget()
self.gridlayout = QGridLayout()
self.scroll = QScrollArea()
self.scroll.setVerticalScrollBarPolicy(Qt.ScrollBarAlwaysOff)
self.scroll.setHorizontalScrollBarPolicy(Qt.ScrollBarAlwaysOff)
self.scroll.setWidgetResizable(True)
font = QFont("Helvetica", 9, QFont.Bold)
### Generate sliders ###
sliders = []
for name in self.jsp.joint_list:
if name not in self.jsp.free_joints:
continue
joint = self.jsp.free_joints[name]
if joint['min'] == joint['max']:
continue
joint_layout = QVBoxLayout()
row_layout = QHBoxLayout()
label = QLabel(name)
label.setFont(font)
row_layout.addWidget(label)
display = QLineEdit("0.00")
display.setAlignment(Qt.AlignRight)
display.setFont(font)
display.setReadOnly(True)
row_layout.addWidget(display)
joint_layout.addLayout(row_layout)
slider = QSlider(Qt.Horizontal)
slider.setFont(font)
slider.setRange(0, RANGE)
slider.setValue(RANGE/2)
joint_layout.addWidget(slider)
self.joint_map[name] = {'slidervalue': 0, 'display': display,
'slider': slider, 'joint': joint}
# Connect to the signal provided by QSignal
slider.valueChanged.connect(self.onValueChanged)
sliders.append(joint_layout)
# Determine number of rows to be used in grid
self.num_rows = num_rows
# if desired num of rows wasn't set, default behaviour is a vertical layout
if self.num_rows == 0:
self.num_rows = len(sliders) # equals VBoxLayout
# Generate positions in grid and place sliders there
self.positions = self.generate_grid_positions(len(sliders), self.num_rows)
for item, pos in zip(sliders, self.positions):
self.gridlayout.addLayout(item, *pos)
# Set zero positions read from parameters
self.center()
# Synchronize slider and displayed value
self.sliderUpdate(None)
# Set up a signal for updating the sliders based on external joint info
self.sliderUpdateTrigger.connect(self.updateSliders)
self.scrollable.setLayout(self.gridlayout)
self.scroll.setWidget(self.scrollable)
self.vlayout.addWidget(self.scroll)
# Buttons for randomizing and centering sliders and
# Spinbox for on-the-fly selecting number of rows
self.ctrbutton = QPushButton('Center', self)
self.ctrbutton.clicked.connect(self.center_event)
self.vlayout.addWidget(self.ctrbutton)
self.setLayout(self.vlayout)
@pyqtSlot(int)
def onValueChanged(self, event):
# A slider value was changed, but we need to change the joint_info metadata.
for name, joint_info in self.joint_map.items():
joint_info['slidervalue'] = joint_info['slider'].value()
joint = joint_info['joint']
joint['position'] = self.sliderToValue(joint_info['slidervalue'], joint)
joint_info['display'].setText("%.2f" % joint['position'])
@pyqtSlot()
def updateSliders(self):
self.update_sliders()
def update_sliders(self):
for name, joint_info in self.joint_map.items():
joint = joint_info['joint']
joint_info['slidervalue'] = self.valueToSlider(joint['position'],
joint)
joint_info['slider'].setValue(joint_info['slidervalue'])
def center_event(self, event):
self.center()
def center(self):
rospy.loginfo("Centering")
for name, joint_info in self.joint_map.items():
joint = joint_info['joint']
joint_info['slider'].setValue(self.valueToSlider(joint['zero'], joint))
def generate_grid_positions(self, num_items, num_rows):
if num_rows==0:
return []
positions = [(y, x) for x in range(int((math.ceil(float(num_items) / num_rows)))) for y in range(num_rows)]
positions = positions[:num_items]
return positions
def sliderUpdate(self, event):
for name, joint_info in self.joint_map.items():
joint_info['slidervalue'] = joint_info['slider'].value()
self.update_sliders()
def valueToSlider(self, value, joint):
return (value - joint['min']) * float(RANGE) / (joint['max'] - joint['min'])
def sliderToValue(self, slider, joint):
pctvalue = slider / float(RANGE)
return joint['min'] + (joint['max']-joint['min']) * pctvalue
class NodeSelectorWidget(QWidget):
_COL_NAMES = ['Node']
# public signal
sig_node_selected = Signal(DynreconfClientWidget)
def __init__(self, parent, rospack, signal_msg=None):
"""
@param signal_msg: Signal to carries a system msg that is shown on GUI.
@type signal_msg: QtCore.Signal
"""
super(NodeSelectorWidget, self).__init__()
self._parent = parent
self.stretch = None
self._signal_msg = signal_msg
ui_file = os.path.join(rospack.get_path('rqt_reconfigure'), 'resource',
'node_selector.ui')
loadUi(ui_file, self)
# List of the available nodes. Since the list should be updated over
# time and we don't want to create node instance per every update
# cycle, This list instance should better be capable of keeping track.
self._nodeitems = OrderedDict()
# Dictionary. 1st elem is node's GRN name,
# 2nd is TreenodeQstdItem instance.
# TODO: Needs updated when nodes list updated.
# Setup treeview and models
self._item_model = TreenodeItemModel()
self._rootitem = self._item_model.invisibleRootItem() # QStandardItem
self._nodes_previous = None
# Calling this method updates the list of the node.
# Initially done only once.
self._update_nodetree_pernode()
# TODO(Isaac): Needs auto-update function enabled, once another
# function that updates node tree with maintaining
# collapse/expansion state. http://goo.gl/GuwYp can be a
# help.
self._collapse_button.pressed.connect(
self._node_selector_view.collapseAll)
self._expand_button.pressed.connect(self._node_selector_view.expandAll)
self._refresh_button.pressed.connect(self._refresh_nodes)
# Filtering preparation.
self._proxy_model = FilterChildrenModel(self)
self._proxy_model.setDynamicSortFilter(True)
self._proxy_model.setSourceModel(self._item_model)
self._node_selector_view.setModel(self._proxy_model)
self._filterkey_prev = ''
# This 1 line is needed to enable horizontal scrollbar. This setting
# isn't available in .ui file.
# Ref. http://stackoverflow.com/a/6648906/577001
try:
self._node_selector_view.header().setResizeMode(
0, QHeaderView.ResizeToContents) # Qt4
except AttributeError:
# TODO QHeaderView.setSectionResizeMode() is currently segfaulting
# using Qt 5 with both bindings PyQt as well as PySide
pass
# Setting slot for when user clicks on QTreeView.
self.selectionModel = self._node_selector_view.selectionModel()
# Note: self.selectionModel.currentChanged doesn't work to deselect
# a treenode as expected. Need to use selectionChanged.
self.selectionModel.selectionChanged.connect(
self._selection_changed_slot)
def node_deselected(self, grn):
"""
Deselect the index that corresponds to the given GRN.
:type grn: str
"""
# Obtain the corresponding index.
qindex_tobe_deselected = self._item_model.get_index_from_grn(grn)
rospy.logdebug('NodeSelWidt node_deselected qindex={} data={}'.format(
qindex_tobe_deselected,
qindex_tobe_deselected.data(Qt.DisplayRole)))
# Obtain all indices currently selected.
indexes_selected = self.selectionModel.selectedIndexes()
for index in indexes_selected:
grn_from_selectedindex = RqtRosGraph.get_upper_grn(index, '')
rospy.logdebug(' Compare given grn={} grn from selected={}'.format(
grn, grn_from_selectedindex))
# If GRN retrieved from selected index matches the given one.
if grn == grn_from_selectedindex:
# Deselect the index.
self.selectionModel.select(index, QItemSelectionModel.Deselect)
def node_selected(self, grn):
"""
Select the index that corresponds to the given GRN.
:type grn: str
"""
# Obtain the corresponding index.
qindex_tobe_selected = self._item_model.get_index_from_grn(grn)
rospy.logdebug('NodeSelWidt node_selected qindex={} data={}'.format(
qindex_tobe_selected, qindex_tobe_selected.data(Qt.DisplayRole)))
# Select the index.
if qindex_tobe_selected:
self.selectionModel.select(qindex_tobe_selected,
QItemSelectionModel.Select)
def _selection_deselected(self, index_current, rosnode_name_selected):
"""
Intended to be called from _selection_changed_slot.
"""
self.selectionModel.select(index_current, QItemSelectionModel.Deselect)
try:
reconf_widget = self._nodeitems[
rosnode_name_selected].get_dynreconf_widget()
except ROSException as e:
raise e
# Signal to notify other pane that also contains node widget.
self.sig_node_selected.emit(reconf_widget)
#self.sig_node_selected.emit(self._nodeitems[rosnode_name_selected])
def _selection_selected(self, index_current, rosnode_name_selected):
"""Intended to be called from _selection_changed_slot."""
rospy.logdebug('_selection_changed_slot row={} col={} data={}'.format(
index_current.row(), index_current.column(),
index_current.data(Qt.DisplayRole)))
# Determine if it's terminal treenode.
found_node = False
for nodeitem in self._nodeitems.values():
name_nodeitem = nodeitem.data(Qt.DisplayRole)
name_rosnode_leaf = rosnode_name_selected[
rosnode_name_selected.rfind(RqtRosGraph.DELIM_GRN) + 1:]
# If name of the leaf in the given name & the name taken from
# nodeitem list matches.
if ((name_nodeitem == rosnode_name_selected) and
(name_nodeitem[name_nodeitem.rfind(RqtRosGraph.DELIM_GRN) + 1:]
== name_rosnode_leaf)):
rospy.logdebug('terminal str {} MATCH {}'.format(
name_nodeitem, name_rosnode_leaf))
found_node = True
break
if not found_node: # Only when it's NOT a terminal we deselect it.
self.selectionModel.select(index_current,
QItemSelectionModel.Deselect)
return
# Only when it's a terminal we move forward.
item_child = self._nodeitems[rosnode_name_selected]
item_widget = None
try:
item_widget = item_child.get_dynreconf_widget()
except ROSException as e:
raise e
rospy.logdebug('item_selected={} child={} widget={}'.format(
index_current, item_child, item_widget))
self.sig_node_selected.emit(item_widget)
# Show the node as selected.
#selmodel.select(index_current, QItemSelectionModel.SelectCurrent)
def _selection_changed_slot(self, selected, deselected):
"""
Sends "open ROS Node box" signal ONLY IF the selected treenode is the
terminal treenode.
Receives args from signal QItemSelectionModel.selectionChanged.
:param selected: All indexs where selected (could be multiple)
:type selected: QItemSelection
:type deselected: QItemSelection
"""
## Getting the index where user just selected. Should be single.
if not selected.indexes() and not deselected.indexes():
rospy.logerr('Nothing selected? Not ideal to reach here')
return
index_current = None
if selected.indexes():
index_current = selected.indexes()[0]
elif len(deselected.indexes()) == 1:
# Setting length criteria as 1 is only a workaround, to avoid
# Node boxes on right-hand side disappears when filter key doesn't
# match them.
# Indeed this workaround leaves another issue. Question for
# permanent solution is asked here http://goo.gl/V4DT1
index_current = deselected.indexes()[0]
rospy.logdebug(' - - - index_current={}'.format(index_current))
rosnode_name_selected = RqtRosGraph.get_upper_grn(index_current, '')
# If retrieved node name isn't in the list of all nodes.
if not rosnode_name_selected in self._nodeitems.keys():
# De-select the selected item.
self.selectionModel.select(index_current,
QItemSelectionModel.Deselect)
return
if selected.indexes():
try:
self._selection_selected(index_current, rosnode_name_selected)
except ROSException as e:
#TODO: print to sysmsg pane
err_msg = e.message + '. Connection to node=' + \
format(rosnode_name_selected) + ' failed'
self._signal_msg.emit(err_msg)
rospy.logerr(err_msg)
elif deselected.indexes():
try:
self._selection_deselected(index_current,
rosnode_name_selected)
except ROSException as e:
rospy.logerr(e.message)
#TODO: print to sysmsg pane
def get_paramitems(self):
"""
:rtype: OrderedDict 1st elem is node's GRN name,
2nd is TreenodeQstdItem instance
"""
return self._nodeitems
def _update_nodetree_pernode(self):
"""
"""
# TODO(Isaac): 11/25/2012 dynamic_reconfigure only returns params that
# are associated with nodes. In order to handle independent
# params, different approach needs taken.
try:
nodes = dyn_reconf.find_reconfigure_services()
except rosservice.ROSServiceIOException as e:
rospy.logerr("Reconfigure GUI cannot connect to master.")
raise e # TODO Make sure 'raise' here returns or finalizes func.
if not nodes == self._nodes_previous:
i_node_curr = 1
num_nodes = len(nodes)
elapsedtime_overall = 0.0
for node_name_grn in nodes:
# Skip this grn if we already have it
if node_name_grn in self._nodeitems:
i_node_curr += 1
continue
time_siglenode_loop = time.time()
####(Begin) For DEBUG ONLY; skip some dynreconf creation
# if i_node_curr % 2 != 0:
# i_node_curr += 1
# continue
#### (End) For DEBUG ONLY. ####
# Instantiate QStandardItem. Inside, dyn_reconf client will
# be generated too.
treenodeitem_toplevel = TreenodeQstdItem(
node_name_grn, TreenodeQstdItem.NODE_FULLPATH)
_treenode_names = treenodeitem_toplevel.get_treenode_names()
try:
treenodeitem_toplevel.connect_param_server()
except rospy.exceptions.ROSException as e:
rospy.logerr(e.message)
#Skip item that fails to connect to its node.
continue
#TODO: Needs to show err msg on GUI too.
# Using OrderedDict here is a workaround for StdItemModel
# not returning corresponding item to index.
self._nodeitems[node_name_grn] = treenodeitem_toplevel
self._add_children_treenode(treenodeitem_toplevel,
self._rootitem, _treenode_names)
time_siglenode_loop = time.time() - time_siglenode_loop
elapsedtime_overall += time_siglenode_loop
_str_progress = 'reconf ' + \
'loading #{}/{} {} / {}sec node={}'.format(
i_node_curr, num_nodes, round(time_siglenode_loop, 2),
round(elapsedtime_overall, 2), node_name_grn)
# NOT a debug print - please DO NOT remove. This print works
# as progress notification when loading takes long time.
rospy.logdebug(_str_progress)
i_node_curr += 1
def _add_children_treenode(self, treenodeitem_toplevel,
treenodeitem_parent, child_names_left):
"""
Evaluate current treenode and the previous treenode at the same depth.
If the name of both nodes is the same, current node instance is
ignored (that means children will be added to the same parent). If not,
the current node gets added to the same parent node. At the end, this
function gets called recursively going 1 level deeper.
:type treenodeitem_toplevel: TreenodeQstdItem
:type treenodeitem_parent: TreenodeQstdItem.
:type child_names_left: List of str
:param child_names_left: List of strings that is sorted in hierarchical
order of params.
"""
# TODO(Isaac): Consider moving this method to rqt_py_common.
name_currentnode = child_names_left.pop(0)
grn_curr = treenodeitem_toplevel.get_raw_param_name()
stditem_currentnode = TreenodeQstdItem(grn_curr,
TreenodeQstdItem.NODE_FULLPATH)
# item at the bottom is your most recent node.
row_index_parent = treenodeitem_parent.rowCount() - 1
# Obtain and instantiate prev node in the same depth.
name_prev = ''
stditem_prev = None
if treenodeitem_parent.child(row_index_parent):
stditem_prev = treenodeitem_parent.child(row_index_parent)
name_prev = stditem_prev.text()
stditem = None
# If the name of both nodes is the same, current node instance is
# ignored (that means children will be added to the same parent)
if name_prev != name_currentnode:
stditem_currentnode.setText(name_currentnode)
# Arrange alphabetically by display name
insert_index = 0
while insert_index < treenodeitem_parent.rowCount(
) and treenodeitem_parent.child(
insert_index).text() < name_currentnode:
insert_index += 1
treenodeitem_parent.insertRow(insert_index, stditem_currentnode)
stditem = stditem_currentnode
else:
stditem = stditem_prev
if child_names_left:
# TODO: Model is closely bound to a certain type of view (treeview)
# here. Ideally isolate those two. Maybe we should split into 2
# class, 1 handles view, the other does model.
self._add_children_treenode(treenodeitem_toplevel, stditem,
child_names_left)
else: # Selectable ROS Node.
#TODO: Accept even non-terminal treenode as long as it's ROS Node.
self._item_model.set_item_from_index(grn_curr, stditem.index())
def _prune_nodetree_pernode(self):
try:
nodes = dyn_reconf.find_reconfigure_services()
except rosservice.ROSServiceIOException as e:
rospy.logerr("Reconfigure GUI cannot connect to master.")
raise e # TODO Make sure 'raise' here returns or finalizes func.
for i in reversed(range(0, self._rootitem.rowCount())):
candidate_for_removal = self._rootitem.child(
i).get_raw_param_name()
if not candidate_for_removal in nodes:
rospy.logdebug(
'Removing {} because the server is no longer available.'.
format(candidate_for_removal))
self._nodeitems[candidate_for_removal].disconnect_param_server(
)
self._rootitem.removeRow(i)
self._nodeitems.pop(candidate_for_removal)
def _refresh_nodes(self):
self._prune_nodetree_pernode()
self._update_nodetree_pernode()
def close_node(self):
rospy.logdebug(" in close_node")
# TODO(Isaac) Figure out if dynamic_reconfigure needs to be closed.
def set_filter(self, filter_):
"""
Pass fileter instance to the child proxymodel.
:type filter_: BaseFilter
"""
self._proxy_model.set_filter(filter_)
def _test_sel_index(self, selected, deselected):
"""
Method for Debug only
"""
#index_current = self.selectionModel.currentIndex()
src_model = self._item_model
index_current = None
index_deselected = None
index_parent = None
curr_qstd_item = None
if selected.indexes():
index_current = selected.indexes()[0]
index_parent = index_current.parent()
curr_qstd_item = src_model.itemFromIndex(index_current)
elif deselected.indexes():
index_deselected = deselected.indexes()[0]
index_parent = index_deselected.parent()
curr_qstd_item = src_model.itemFromIndex(index_deselected)
if selected.indexes() > 0:
rospy.logdebug('sel={} par={} desel={} sel.d={} par.d={}'.format(
index_current,
index_parent,
index_deselected,
index_current.data(Qt.DisplayRole),
index_parent.data(Qt.DisplayRole),
) + ' desel.d={} cur.item={}'.format(
None, # index_deselected.data(Qt.DisplayRole)
curr_qstd_item))
elif deselected.indexes():
rospy.logdebug(
'sel={} par={} desel={} sel.d={} par.d={}'.format(
index_current, index_parent, index_deselected, None,
index_parent.data(Qt.DisplayRole)) +
' desel.d={} cur.item={}'.format(
index_deselected.data(Qt.DisplayRole), curr_qstd_item))
class SpyderShellWidget(ExternalShellBase):
"""Spyder Shell Widget: execute a shell in a separate process using spyderlib's ExternalShellBase"""
SHELL_CLASS = TerminalWidget
close_signal = Signal()
def __init__(self, parent=None):
ExternalShellBase.__init__(self, parent=parent, fname=None, wdir='.',
history_filename='.history',
light_background=True,
menu_actions=None,
show_buttons_inside=False,
show_elapsed_time=False)
self.setObjectName('SpyderShellWidget')
# capture tab key
#self.shell._key_tab = self._key_tab
self.shell.set_pythonshell_font(QFont('Mono'))
# Additional python path list
self.path = []
# For compatibility with the other shells that can live in the external console
self.is_ipython_kernel = False
self.connection_file = None
self.create_process()
def get_icon(self):
return QIcon()
def create_process(self):
self.shell.clear()
self.process = QProcess(self)
self.process.setProcessChannelMode(QProcess.MergedChannels)
env = []
for key_val_pair in self.process.systemEnvironment():
try:
value = unicode(key_val_pair)
except NameError:
value = str(key_val_pair)
env.append(value)
env.append('TERM=xterm')
env.append('COLORTERM=gnome-terminal')
self.process.setEnvironment(env)
# Working directory
if self.wdir is not None:
self.process.setWorkingDirectory(self.wdir)
self.process.readyReadStandardOutput.connect(self.write_output)
self.process.finished.connect(self.finished)
self.process.finished.connect(self.close_signal)
self.process.start('/bin/bash', ['-i'])
running = self.process.waitForStarted()
self.set_running_state(running)
if not running:
self.shell.addPlainText("Process failed to start")
else:
self.shell.setFocus()
self.emit(SIGNAL('started()'))
return self.process
def shutdown(self):
self.process.kill()
self.process.waitForFinished()
def _key_tab(self):
self.process.write('\t')
self.process.waitForBytesWritten(-1)
self.write_output()
def send_to_process(self, text):
if not is_string(text):
try:
text = unicode(text)
except NameError:
text = str(text)
if not text.endswith('\n'):
text += '\n'
self.process.write(QTextCodec.codecForLocale().fromUnicode(text))
self.process.waitForBytesWritten(-1)
def keyboard_interrupt(self):
self.send_ctrl_to_process('c')
class DynamicTimeline(QGraphicsScene):
"""
BagTimeline contains bag files, all information required to display the bag data visualization on the screen
Also handles events
"""
status_bar_changed_signal = Signal()
selected_region_changed = Signal(rospy.Time, rospy.Time)
timeline_updated = Signal()
Topic = collections.namedtuple('Topic', ['subscriber', 'queue'])
Message = collections.namedtuple('Message', ['stamp', 'message'])
def __init__(self, context, publish_clock):
"""
:param context: plugin context hook to enable adding rqt_bag plugin widgets as ROS_GUI snapin panes, ''PluginContext''
"""
super(DynamicTimeline, self).__init__()
# key is topic name, value is a named tuple of type Topic. The deque
# contains named tuples of type Message
self._topics = {}
# key is the data type, value is a list of topics with that type
self._datatypes = {}
self._topic_lock = threading.RLock()
self.background_task = None # Display string
self.background_task_cancel = False
# Playing / Recording
self._playhead_lock = threading.RLock()
self._max_play_speed = 1024.0 # fastest X play speed
self._min_play_speed = 1.0 / 1024.0 # slowest X play speed
self._play_speed = 0.0
self._play_all = False
self._playhead_positions_cvs = {}
self._playhead_positions = {} # topic -> position
self._message_loaders = {}
self._messages_cvs = {}
self._messages = {} # topic -> msg_data
self._message_listener_threads = {
} # listener -> MessageListenerThread
self._player = False
self._publish_clock = publish_clock
self._recorder = None
self.last_frame = None
self.last_playhead = None
self.desired_playhead = None
self.wrap = True # should the playhead wrap when it reaches the end?
self.stick_to_end = False # should the playhead stick to the end?
self._play_timer = QTimer()
self._play_timer.timeout.connect(self.on_idle)
self._play_timer.setInterval(3)
self._redraw_timer = None # timer which can be used to periodically redraw the timeline
# Plugin popup management
self._context = context
self.popups = {}
self._views = []
self._listeners = {}
# Initialize scene
# the timeline renderer fixes use of black pens and fills, so ensure we fix white here for contrast.
# otherwise a dark qt theme will default it to black and the frame render pen will be unreadable
self.setBackgroundBrush(Qt.white)
self._timeline_frame = DynamicTimelineFrame(self)
self._timeline_frame.setPos(0, 0)
self.addItem(self._timeline_frame)
self.background_progress = 0
self.__closed = False
# timer to periodically redraw the timeline every so often
self._start_redraw_timer()
def get_context(self):
"""
:returns: the ROS_GUI context, 'PluginContext'
"""
return self._context
def _start_redraw_timer(self):
if not self._redraw_timer:
self._redraw_timer = rospy.Timer(rospy.Duration(0.5),
self._redraw_timeline)
def _stop_redraw_timer(self):
if self._redraw_timer:
self._redraw_timer.shutdown()
self._redraw_timer = None
def handle_close(self):
"""
Cleans up the timeline, subscribers and any threads
"""
if self.__closed:
return
else:
self.__closed = True
self._play_timer.stop()
for topic in self._get_topics():
self.stop_publishing(topic)
self._message_loaders[topic].stop()
if self._player:
self._player.stop()
if self._recorder:
self._recorder.stop()
if self.background_task is not None:
self.background_task_cancel = True
self._timeline_frame.handle_close()
for topic in self._topics:
self._topics[topic][0].unregister() # unregister the subscriber
for frame in self._views:
if frame.parent():
self._context.remove_widget(frame)
def _redraw_timeline(self, timer):
# save the playhead so that the redraw doesn't move it
playhead = self._timeline_frame._playhead
if playhead is None:
start = self._timeline_frame.play_region[0] is None
end = self._timeline_frame.play_region[1] is None
else:
start = True if playhead <= self._timeline_frame.play_region[
0] else False
end = True if playhead >= self._timeline_frame.play_region[
1] else False
# do not keep setting this if you want the timeline to just grow.
self._timeline_frame._start_stamp = self._get_start_stamp()
self._timeline_frame._end_stamp = self._get_end_stamp()
self._timeline_frame.reset_zoom()
if end:
# use play region instead of time.now to stop playhead going past
# the end of the region, which causes problems with
# navigate_previous
self._timeline_frame._set_playhead(
self._timeline_frame.play_region[1])
elif start:
self._timeline_frame._set_playhead(
self._timeline_frame.play_region[0])
else:
if playhead:
self._timeline_frame._set_playhead(playhead)
self.timeline_updated.emit()
def topic_callback(self, msg, topic):
"""
Called whenever a message is received on any of the subscribed topics
:param topic: the topic on which the message was received
:param msg: the message received
"""
message = self.Message(stamp=rospy.Time.now(), message=msg)
with self._topic_lock:
self._topics[topic].queue.append(message)
# Invalidate entire cache for this topic
with self._timeline_frame.index_cache_cv:
self._timeline_frame.invalidated_caches.add(topic)
# if topic in self._timeline_frame.index_cache:
# del self._timeline_frame.index_cache[topic]
self._timeline_frame.index_cache_cv.notify()
def add_topic(self, topic, type, num_msgs=20):
"""Creates an indexing thread for the new topic. Fixes the borders and notifies
the indexing thread to index the new items bags
:param topic: a topic to listen to
:param type: type of the topic to listen to
:param num_msgs: number of messages to retain on this topic. If this
value is exceeded, the oldest messages will be dropped
:return: false if topic already in the timeline, true otherwise
"""
# first item in each sub-list is the name, second is type
if topic not in self._topics:
self._topics[topic] = self.Topic(
subscriber=rospy.Subscriber(topic,
type,
queue_size=1,
callback=self.topic_callback,
callback_args=topic),
queue=collections.deque(maxlen=num_msgs))
self._datatypes.setdefault(type, []).append(topic)
else:
return False
self._playhead_positions_cvs[topic] = threading.Condition()
self._messages_cvs[topic] = threading.Condition()
self._message_loaders[topic] = MessageLoaderThread(self, topic)
self._timeline_frame._start_stamp = self._get_start_stamp()
self._timeline_frame._end_stamp = self._get_end_stamp()
self._timeline_frame.topics = self._get_topics()
self._timeline_frame._topics_by_datatype = self._get_topics_by_datatype(
)
# If this is the first bag, reset the timeline
self._timeline_frame.reset_zoom()
# Invalidate entire cache for this topic
with self._timeline_frame.index_cache_cv:
self._timeline_frame.invalidated_caches.add(topic)
if topic in self._timeline_frame.index_cache:
del self._timeline_frame.index_cache[topic]
self._timeline_frame.index_cache_cv.notify()
return True
# TODO Rethink API and if these need to be visible
def _get_start_stamp(self):
"""
:return: stamp of the first message received on any of the topics, or none if no messages have been received, ''rospy.Time''
"""
with self._topic_lock:
start_stamp = None
for unused_topic_name, topic_tuple in self._topics.items():
topic_start_stamp = topic_helper.get_start_stamp(topic_tuple)
if topic_start_stamp is not None and (
start_stamp is None
or topic_start_stamp < start_stamp):
start_stamp = topic_start_stamp
return start_stamp
def _get_end_stamp(self):
"""
:return: stamp of the last message received on any of the topics, or none if no messages have been received, ''rospy.Time''
"""
with self._topic_lock:
end_stamp = None
for unused_topic_name, topic_tuple in self._topics.items():
topic_end_stamp = topic_helper.get_end_stamp(topic_tuple)
if topic_end_stamp is not None and (
end_stamp is None or topic_end_stamp > end_stamp):
end_stamp = topic_end_stamp
return end_stamp
def _get_topics(self):
"""
:return: sorted list of topic names, ''list(str)''
"""
with self._topic_lock:
topics = []
for topic in self._topics:
topics.append(topic)
return sorted(topics)
def _get_topics_by_datatype(self):
"""
:return: dict of list of topics for each datatype, ''dict(datatype:list(topic))''
"""
with self._topic_lock:
return self._datatypes
def get_datatype(self, topic):
"""
:return: datatype associated with a topic, ''str''
:raises: if there are multiple datatypes assigned to a single topic, ''Exception''
"""
with self._topic_lock:
topic_types = []
for datatype in self._datatypes:
if topic in self._datatypes[datatype]:
if len(topic_types) == 1:
raise Exception(
"Topic {0} had multiple datatypes ({1}) associated with it"
.format(topic, str(topic_types)))
topic_types.append(datatype._type)
if not topic_types:
return None
else:
return topic_types[0]
def get_entries(self, topics, start_stamp, end_stamp):
"""
generator function for topic entries
:param topics: list of topics to query, ''list(str)''
:param start_stamp: stamp to start at, ''rospy.Time''
:param end_stamp: stamp to end at, ''rospy,Time''
:returns: messages on the given topics in chronological order, ''msg''
"""
with self._topic_lock:
topic_entries = []
# make sure that we can handle a single topic as well
for topic in topics:
if not topic in self._topics:
rospy.logwarn(
"Dynamic Timeline : Topic {0} was not in the topic list. Skipping."
.format(topic))
continue
# don't bother with topics if they don't overlap the requested time range
topic_start_time = topic_helper.get_start_stamp(
self._topics[topic])
if topic_start_time is not None and topic_start_time > end_stamp:
continue
topic_end_time = topic_helper.get_end_stamp(
self._topics[topic])
if topic_end_time is not None and topic_end_time < start_stamp:
continue
topic_queue = self._topics[topic].queue
start_ind, first_entry = self._entry_at(
start_stamp, topic_queue)
# entry returned might be the latest one before the start stamp
# if there isn't one exactly on the stamp - if so, start from
# the next entry
if first_entry.stamp < start_stamp:
start_ind += 1
# entry at always returns entry at or before the given stamp, so
# no manipulation needed.
end_ind, last_entry = self._entry_at(end_stamp, topic_queue)
topic_entries.extend(
list(itertools.islice(topic_queue, start_ind,
end_ind + 1)))
for entry in sorted(topic_entries, key=lambda x: x.stamp):
yield entry
def get_entries_with_bags(self, topic, start_stamp, end_stamp):
"""
generator function for bag entries
:param topics: list of topics to query, ''list(str)''
:param start_stamp: stamp to start at, ''rospy.Time''
:param end_stamp: stamp to end at, ''rospy,Time''
:returns: tuple of (bag, entry) for the entries in the bag file, ''(rosbag.bag, msg)''
"""
with self._bag_lock:
from rosbag import bag # for _mergesort
bag_entries = []
bag_by_iter = {}
for b in self._bags:
bag_start_time = bag_helper.get_start_stamp(b)
if bag_start_time is not None and bag_start_time > end_stamp:
continue
bag_end_time = bag_helper.get_end_stamp(b)
if bag_end_time is not None and bag_end_time < start_stamp:
continue
connections = list(b._get_connections(topic))
it = iter(b._get_entries(connections, start_stamp, end_stamp))
bag_by_iter[it] = b
bag_entries.append(it)
for entry, it in bag._mergesort(bag_entries,
key=lambda entry: entry.time):
yield bag_by_iter[it], entry
def _entry_at(self, t, queue):
"""Get the entry and index in the queue at the given time.
:param ``rospy.Time`` t: time to check
:param ``collections.deque`` queue: deque to look at
:return: (index, Message) tuple. If there is no message in the queue at
the exact time, the previous index is returned. If the time is
before or after the first and last message times, the first or last
index and message are returned
"""
# Gives the index to insert into to retain a sorted queue. The topic queues
# should always be sorted due to time passing.
# ind = bisect.bisect(queue, self.Message(stamp=t, message=''))
# Can't use bisect here in python3 unless the correct operators
# are defined for sorting, so do it manually
try:
ind = next(i for i, msg in enumerate(queue) if t < msg.stamp)
except StopIteration:
ind = len(queue)
# first or last indices
if ind == len(queue):
return (ind - 1, queue[-1])
elif ind == 0:
return (0, queue[0])
# non-end indices
cur = queue[ind]
if cur.stamp == t:
return (ind, cur)
else:
return (ind - 1, queue[ind - 1])
def get_entry(self, t, topic):
"""Get a message in the queues for a specific topic
:param ``rospy.Time`` t: time of the message to retrieve
:param str topic: the topic to be accessed
:return: message corresponding to time t and topic. If there is no
corresponding entry at exactly the given time, the latest entry
before the given time is returned. If the topic does not exist, or
there is no entry, None.
"""
with self._topic_lock:
entry = None
if topic in self._topics:
_, entry = self._entry_at(t, self._topics[topic].queue)
return entry
def get_entry_before(self, t):
"""
Get the latest message before the given time on any of the topics
:param t: time, ``rospy.Time``
:return: tuple of (topic, entry) corresponding to time t, ``(str, msg)``
"""
with self._topic_lock:
entry_topic, entry = None, None
for topic in self._topics:
_, topic_entry = self._entry_at(t - rospy.Duration(0, 1),
self._topics[topic].queue)
if topic_entry and (not entry
or topic_entry.stamp > entry.stamp):
entry_topic, entry = topic, topic_entry
return entry_topic, entry
def get_entry_after(self, t):
"""
Get the earliest message on any topic after the given time
:param t: time, ''rospy.Time''
:return: tuple of (bag, entry) corresponding to time t, ''(rosbag.bag, msg)''
"""
with self._topic_lock:
entry_topic, entry = None, None
for topic in self._topics:
ind, _ = self._entry_at(t, self._topics[topic].queue)
# ind is the index of the entry at (if it exists) or before time
# t - we want the one after this. Make sure that the given index
# isn't out of bounds
ind = ind + 1 if ind + 1 < len(
self._topics[topic].queue) else ind
topic_entry = self._topics[topic].queue[ind]
if topic_entry and (not entry
or topic_entry.stamp < entry.stamp):
entry_topic, entry = topic, topic_entry
return entry_topic, entry
def get_next_message_time(self):
"""
:return: time of the message after the current playhead position,''rospy.Time''
"""
if self._timeline_frame.playhead is None:
return None
_, entry = self.get_entry_after(self._timeline_frame.playhead)
if entry is None:
return self._timeline_frame._end_stamp
return entry.stamp
def get_previous_message_time(self):
"""
:return: time of the message before the current playhead position,''rospy.Time''
"""
if self._timeline_frame.playhead is None:
return None
_, entry = self.get_entry_before(self._timeline_frame.playhead)
if entry is None:
return self._timeline_frame._start_stamp
return entry.stamp
def resume(self):
if (self._player):
self._player.resume()
### Copy messages to...
def start_background_task(self, background_task):
"""
Verify that a background task is not currently running before starting a new one
:param background_task: name of the background task, ''str''
"""
if self.background_task is not None:
QMessageBox(
QMessageBox.Warning, 'Exclamation',
'Background operation already running:\n\n%s' %
self.background_task, QMessageBox.Ok).exec_()
return False
self.background_task = background_task
self.background_task_cancel = False
return True
def stop_background_task(self):
self.background_task = None
def copy_region_to_bag(self, filename):
if len(self._bags) > 0:
self._export_region(filename, self._timeline_frame.topics,
self._timeline_frame.play_region[0],
self._timeline_frame.play_region[1])
def _export_region(self, path, topics, start_stamp, end_stamp):
"""
Starts a thread to save the current selection to a new bag file
:param path: filesystem path to write to, ''str''
:param topics: topics to write to the file, ''list(str)''
:param start_stamp: start of area to save, ''rospy.Time''
:param end_stamp: end of area to save, ''rospy.Time''
"""
if not self.start_background_task('Copying messages to "%s"' % path):
return
# TODO implement a status bar area with information on the current save status
bag_entries = list(
self.get_entries_with_bags(topics, start_stamp, end_stamp))
if self.background_task_cancel:
return
# Get the total number of messages to copy
total_messages = len(bag_entries)
# If no messages, prompt the user and return
if total_messages == 0:
QMessageBox(QMessageBox.Warning, 'rqt_bag', 'No messages found',
QMessageBox.Ok).exec_()
self.stop_background_task()
return
# Open the path for writing
try:
export_bag = rosbag.Bag(path, 'w')
except Exception:
QMessageBox(QMessageBox.Warning, 'rqt_bag',
'Error opening bag file [%s] for writing' % path,
QMessageBox.Ok).exec_()
self.stop_background_task()
return
# Run copying in a background thread
self._export_thread = threading.Thread(target=self._run_export_region,
args=(export_bag, topics,
start_stamp, end_stamp,
bag_entries))
self._export_thread.start()
def _run_export_region(self, export_bag, topics, start_stamp, end_stamp,
bag_entries):
"""
Threaded function that saves the current selection to a new bag file
:param export_bag: bagfile to write to, ''rosbag.bag''
:param topics: topics to write to the file, ''list(str)''
:param start_stamp: start of area to save, ''rospy.Time''
:param end_stamp: end of area to save, ''rospy.Time''
"""
total_messages = len(bag_entries)
update_step = max(1, total_messages / 100)
message_num = 1
progress = 0
# Write out the messages
for bag, entry in bag_entries:
if self.background_task_cancel:
break
try:
topic, msg, t = self.read_message(bag, entry.position)
export_bag.write(topic, msg, t)
except Exception as ex:
qWarning('Error exporting message at position %s: %s' %
(str(entry.position), str(ex)))
export_bag.close()
self.stop_background_task()
return
if message_num % update_step == 0 or message_num == total_messages:
new_progress = int(100.0 *
(float(message_num) / total_messages))
if new_progress != progress:
progress = new_progress
if not self.background_task_cancel:
self.background_progress = progress
self.status_bar_changed_signal.emit()
message_num += 1
# Close the bag
try:
self.background_progress = 0
self.status_bar_changed_signal.emit()
export_bag.close()
except Exception as ex:
QMessageBox(
QMessageBox.Warning, 'rqt_bag',
'Error closing bag file [%s]: %s' %
(export_bag.filename, str(ex)), QMessageBox.Ok).exec_()
self.stop_background_task()
def read_message(self, topic, position):
with self._topic_lock:
return self.get_entry(position, topic).message
def on_mouse_down(self, event):
"""
When the user clicks down in the timeline.
"""
if event.buttons() == Qt.LeftButton:
self._timeline_frame.on_left_down(event)
elif event.buttons() == Qt.MidButton:
self._timeline_frame.on_middle_down(event)
elif event.buttons() == Qt.RightButton:
topic = self._timeline_frame.map_y_to_topic(event.y())
TimelinePopupMenu(self, event, topic)
def on_mouse_up(self, event):
self._timeline_frame.on_mouse_up(event)
def on_mouse_move(self, event):
self._timeline_frame.on_mouse_move(event)
def on_mousewheel(self, event):
self._timeline_frame.on_mousewheel(event)
# Zooming
def zoom_in(self):
self._timeline_frame.zoom_in()
def zoom_out(self):
self._timeline_frame.zoom_out()
def reset_zoom(self):
self._timeline_frame.reset_zoom()
def translate_timeline_left(self):
self._timeline_frame.translate_timeline_left()
def translate_timeline_right(self):
self._timeline_frame.translate_timeline_right()
### Publishing
def is_publishing(self, topic):
return self._player and self._player.is_publishing(topic)
def start_publishing(self, topic):
if not self._player and not self._create_player():
return False
self._player.start_publishing(topic)
return True
def stop_publishing(self, topic):
if not self._player:
return False
self._player.stop_publishing(topic)
return True
def _create_player(self):
if not self._player:
try:
self._player = Player(self)
if self._publish_clock:
self._player.start_clock_publishing()
except Exception as ex:
qWarning('Error starting player; aborting publish: %s' %
str(ex))
return False
return True
def set_publishing_state(self, start_publishing):
if start_publishing:
for topic in self._timeline_frame.topics:
if not self.start_publishing(topic):
break
else:
for topic in self._timeline_frame.topics:
self.stop_publishing(topic)
# property: play_all
def _get_play_all(self):
return self._play_all
def _set_play_all(self, play_all):
if play_all == self._play_all:
return
self._play_all = not self._play_all
self.last_frame = None
self.last_playhead = None
self.desired_playhead = None
play_all = property(_get_play_all, _set_play_all)
def toggle_play_all(self):
self.play_all = not self.play_all
### Playing
def on_idle(self):
self._step_playhead()
def _step_playhead(self):
"""
moves the playhead to the next position based on the desired position
"""
# Reset when the playing mode switchs
if self._timeline_frame.playhead != self.last_playhead:
self.last_frame = None
self.last_playhead = None
self.desired_playhead = None
if self._play_all:
self.step_next_message()
else:
self.step_fixed()
def step_fixed(self):
"""
Moves the playhead a fixed distance into the future based on the current play speed
"""
if self.play_speed == 0.0 or not self._timeline_frame.playhead:
self.last_frame = None
self.last_playhead = None
return
now = rospy.Time.from_sec(time.time())
if self.last_frame:
# Get new playhead
if self.stick_to_end:
new_playhead = self.end_stamp
else:
new_playhead = self._timeline_frame.playhead + rospy.Duration.from_sec(
(now - self.last_frame).to_sec() * self.play_speed)
start_stamp, end_stamp = self._timeline_frame.play_region
if new_playhead > end_stamp:
if self.wrap:
if self.play_speed > 0.0:
new_playhead = start_stamp
else:
new_playhead = end_stamp
else:
new_playhead = end_stamp
if self.play_speed > 0.0:
self.stick_to_end = True
elif new_playhead < start_stamp:
if self.wrap:
if self.play_speed < 0.0:
new_playhead = end_stamp
else:
new_playhead = start_stamp
else:
new_playhead = start_stamp
# Update the playhead
self._timeline_frame.playhead = new_playhead
self.last_frame = now
self.last_playhead = self._timeline_frame.playhead
def step_next_message(self):
"""
Move the playhead to the next message
"""
if self.play_speed <= 0.0 or not self._timeline_frame.playhead:
self.last_frame = None
self.last_playhead = None
return
if self.last_frame:
if not self.desired_playhead:
self.desired_playhead = self._timeline_frame.playhead
else:
delta = rospy.Time.from_sec(time.time()) - self.last_frame
if delta > rospy.Duration.from_sec(0.1):
delta = rospy.Duration.from_sec(0.1)
self.desired_playhead += delta
# Get the occurrence of the next message
next_message_time = self.get_next_message_time()
if next_message_time < self.desired_playhead:
self._timeline_frame.playhead = next_message_time
else:
self._timeline_frame.playhead = self.desired_playhead
self.last_frame = rospy.Time.from_sec(time.time())
self.last_playhead = self._timeline_frame.playhead
# Recording
def record_bag(self, filename, all=True, topics=[], regex=False, limit=0):
try:
self._recorder = Recorder(filename,
bag_lock=self._bag_lock,
all=all,
topics=topics,
regex=regex,
limit=limit)
except Exception as ex:
qWarning('Error opening bag for recording [%s]: %s' %
(filename, str(ex)))
return
self._recorder.add_listener(self._message_recorded)
self.add_bag(self._recorder.bag)
self._recorder.start()
self.wrap = False
self._timeline_frame._index_cache_thread.period = 0.1
self.update()
def toggle_recording(self):
if self._recorder:
self._recorder.toggle_paused()
self.update()
def _message_recorded(self, topic, msg, t):
if self._timeline_frame._start_stamp is None:
self._timeline_frame._start_stamp = t
self._timeline_frame._end_stamp = t
self._timeline_frame._playhead = t
elif self._timeline_frame._end_stamp is None or t > self._timeline_frame._end_stamp:
self._timeline_frame._end_stamp = t
if not self._timeline_frame.topics or topic not in self._timeline_frame.topics:
self._timeline_frame.topics = self._get_topics()
self._timeline_frame._topics_by_datatype = self._get_topics_by_datatype(
)
self._playhead_positions_cvs[topic] = threading.Condition()
self._messages_cvs[topic] = threading.Condition()
self._message_loaders[topic] = MessageLoaderThread(self, topic)
if self._timeline_frame._stamp_left is None:
self.reset_zoom()
# Notify the index caching thread that it has work to do
with self._timeline_frame.index_cache_cv:
self._timeline_frame.invalidated_caches.add(topic)
self._timeline_frame.index_cache_cv.notify()
if topic in self._listeners:
for listener in self._listeners[topic]:
try:
listener.timeline_changed()
except Exception as ex:
qWarning('Error calling timeline_changed on %s: %s' %
(type(listener), str(ex)))
# Views / listeners
def add_view(self, topic, frame):
self._views.append(frame)
def has_listeners(self, topic):
return topic in self._listeners
def add_listener(self, topic, listener):
self._listeners.setdefault(topic, []).append(listener)
self._message_listener_threads[(topic,
listener)] = MessageListenerThread(
self, topic, listener)
# Notify the message listeners
self._message_loaders[topic].reset()
with self._playhead_positions_cvs[topic]:
self._playhead_positions_cvs[topic].notify_all()
self.update()
def remove_listener(self, topic, listener):
topic_listeners = self._listeners.get(topic)
if topic_listeners is not None and listener in topic_listeners:
topic_listeners.remove(listener)
if len(topic_listeners) == 0:
del self._listeners[topic]
# Stop the message listener thread
if (topic, listener) in self._message_listener_threads:
self._message_listener_threads[(topic, listener)].stop()
del self._message_listener_threads[(topic, listener)]
self.update()
### Playhead
# property: play_speed
def _get_play_speed(self):
if self._timeline_frame._paused:
return 0.0
return self._play_speed
def _set_play_speed(self, play_speed):
if play_speed == self._play_speed:
return
if play_speed > 0.0:
self._play_speed = min(self._max_play_speed,
max(self._min_play_speed, play_speed))
elif play_speed < 0.0:
self._play_speed = max(-self._max_play_speed,
min(-self._min_play_speed, play_speed))
else:
self._play_speed = play_speed
if self._play_speed < 1.0:
self.stick_to_end = False
self.update()
play_speed = property(_get_play_speed, _set_play_speed)
def toggle_play(self):
if self._play_speed != 0.0:
self.play_speed = 0.0
else:
self.play_speed = 1.0
def navigate_play(self):
self.play_speed = 1.0
self.last_frame = rospy.Time.from_sec(time.time())
self.last_playhead = self._timeline_frame.playhead
self._play_timer.start()
def navigate_stop(self):
self.play_speed = 0.0
self._play_timer.stop()
def navigate_previous(self):
self.navigate_stop()
self._timeline_frame.playhead = self.get_previous_message_time()
self.last_playhead = self._timeline_frame.playhead
def navigate_next(self):
self.navigate_stop()
self._timeline_frame.playhead = self.get_next_message_time()
self.last_playhead = self._timeline_frame.playhead
def navigate_rewind(self):
if self._play_speed < 0.0:
new_play_speed = self._play_speed * 2.0
elif self._play_speed == 0.0:
new_play_speed = -1.0
else:
new_play_speed = self._play_speed * 0.5
self.play_speed = new_play_speed
def navigate_fastforward(self):
if self._play_speed > 0.0:
new_play_speed = self._play_speed * 2.0
elif self._play_speed == 0.0:
new_play_speed = 2.0
else:
new_play_speed = self._play_speed * 0.5
self.play_speed = new_play_speed
def navigate_start(self):
self._timeline_frame.playhead = self._timeline_frame.play_region[0]
def navigate_end(self):
self._timeline_frame.playhead = self._timeline_frame.play_region[1]
class QtRappManager(Plugin):
_update_rapps_signal = Signal()
def __init__(self, context):
self._context = context
super(QtRappManager, self).__init__(context)
self._init_ui(context)
self._init_events()
self._init_variables()
self.spin()
def _init_ui(self, context):
self._widget = QWidget()
self.setObjectName('QtRappManger')
rospack = rospkg.RosPack()
ui_file = os.path.join(rospack.get_path('rocon_qt_app_manager'), 'ui',
'qt_rapp_manager.ui')
self._widget.setObjectName('QtRappManger')
loadUi(ui_file, self._widget, {})
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
context.add_widget(self._widget)
# Set view mode
self._widget.rapp_grid.setViewMode(QListView.IconMode)
# self._widget.rapp_grid.setViewMode(QListView.ListMode)
def _init_events(self):
# combo box change event
self._widget.namespace_cbox.currentIndexChanged.connect(
self._change_namespace)
# Rapp single click event
self._widget.rapp_grid.clicked.connect(self._rapp_single_click)
# Rapp double click event
self._widget.rapp_grid.doubleClicked.connect(self._rapp_double_click)
def _init_variables(self):
self.initialised = False
# init
self._qt_rapp_manager_info = QtRappManagerInfo(self._refresh_rapps)
self._update_rapps_signal.connect(self._update_rapp_list)
self._rapp_view_model = QStandardItemModel()
self._widget.rapp_grid.setModel(self._rapp_view_model)
self._widget.rapp_grid.setWrapping(True)
self._widget.rapp_grid.setIconSize(QSize(60, 60))
self._widget.rapp_grid.setSpacing(10)
self._selected_rapp = None
def spin(self):
self._get_appmanager_namespaces()
def _cleanup_rapps(self):
self._rapp_view_model.clear()
def _get_appmanager_namespaces(self):
namespaces = self._qt_rapp_manager_info._get_namespaces()
for namespace in namespaces:
ns = namespace[:namespace.find('list_rapps')]
self._widget.namespace_cbox.addItem(ns)
def _exit(self):
pass
###################################################################
# Events
###################################################################
def _change_namespace(self, event):
self._qt_rapp_manager_info.select_rapp_manager(
self._widget.namespace_cbox.currentText())
def _refresh_rapps(self):
"""
Updates from qt_app_manager_info.
"""
self._update_rapps_signal.emit()
def _update_rapp_list(self):
"""
Rapp manager namespace event
"""
self._cleanup_rapps()
available_rapps = self._qt_rapp_manager_info.get_available_rapps()
running_rapps = self._qt_rapp_manager_info.get_running_rapps()
for r, v in available_rapps.items():
if r in running_rapps.keys():
item = QRappItem(v, running=True)
else:
item = QRappItem(v, running=False)
self._rapp_view_model.appendRow(item)
def _rapp_single_click(self, index):
qrapp = self._rapp_view_model.item(index.row())
rapp = qrapp.getRapp()
self._create_rapp_dialog(rapp)
def _create_rapp_dialog(self, rapp):
is_running = self._qt_rapp_manager_info.is_running_rapp(rapp)
self._selected_rapp = rapp
self._dialog = QtRappDialog(self._widget, rapp,
self._qt_rapp_manager_info.start_rapp,
self._qt_rapp_manager_info.stop_rapp,
is_running)
self._dialog.show()
def _rapp_double_click(self, item):
running_rapps = self._qt_rapp_manager_info.get_running_rapps()
if len(running_rapps) > 0:
names = [r['display_name'] for r in running_rapps.values()]
show_message(self._widget, "Error",
"Rapp %s are already running" % names)
else:
self._start_rapp()
def _start_rapp(self):
result = self._qt_rapp_manager_info.start_rapp(
self._selected_rapp['name'], [],
self._selected_rapp['public_parameters'])
show_message(self._widget, str(result.started), result.message)
self._selected_rapp = None
return result
def _stop_rapp(self):
result = self._qt_rapp_manager_info.stop_rapp()
show_message(self._widget, str(result.stopped), result.message)
self._selected_rapp = None
return result
class ClassName(QObject):
_update_task_delegates = Signal()
_update_endeffector_widget = Signal()
def __init__(self, context):
QObject.__init__(self, context)
self.setObjectName('ClassName')
self.reward_total = 0
self.execute_policy = False
self.record_button = False
self.save_button = False
self.plot_button = False
self.t = 0
self.pos_car1 = []
self.pos_car2 = []
self.trajectory_collector = []
self.listener = tf.TransformListener()
self.car1_goal = [38., 3.]
#self.car2_goal = [38., 3.]
self.car2_goal = [30., 32.]
# setup publisher
# car 1
self._examplePublisher = rospy.Publisher('/policy_gui/exampletopic', Bool,queue_size=10)
self.goal_client1 = actionlib.SimpleActionClient('/car1/move_base', MoveBaseAction)
self.goal_client1.wait_for_server()
self.velocity_service1_ = rospy.ServiceProxy('/car1/car_control/pomdp_velocity', ActionObservation)
#self.listener = tf.TransformListener()
# car 2
self._examplePublisher = rospy.Publisher('/policy_gui/exampletopic', Bool,queue_size=10)
self.goal_client2 = actionlib.SimpleActionClient('/car2/move_base', MoveBaseAction)
self.goal_client2.wait_for_server()
self.velocity_service2_ = rospy.ServiceProxy('/car2/car_control/pomdp_velocity', ActionObservation)
# self.listener = tf.TransformListener()
# setup services
#self.getObjects = rospy.ServiceProxy('worldmodel/get_object_model', GetObjectModel)
# setup subscribers
#self._worldmodelObjectsSubscriber = rospy.Subscriber("/worldmodel/objects", ObjectModel, self._on_worldmodel_objects)
#self._interactive_marker_endeffector = rospy.Subscriber("/interactive_marker_pose_control/feedback",InteractiveMarkerFeedback,
#self._on_interactive_marker_endeffector_pose,queue_size=1)
# setup action clients
#self._move_arm_client = actionlib.SimpleActionClient("/move_group", MoveGroupAction)
#setup tf listener
#self.tf_listener = TransformListener()
# setup main widget
self._widget = QWidget()
ui_file = os.path.join(rospkg.RosPack().get_path('policy_gui'), 'lib', 'ClassName.ui')
loadUi(ui_file, self._widget)
self._widget.setObjectName('ClassNameUi')
#set connections
self._widget.start_button.pressed.connect(self._on_start_button_pressed)
self._widget.setup_button.pressed.connect(self._on_setup_button_pressed)
self._widget.record_button.pressed.connect(self._on_record_button_pressed)
self._widget.save_button.pressed.connect(self._on_save_button_pressed)
self._widget.plot_button.pressed.connect(self._on_plot_button_pressed)
#getRobotJoints_button
# 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.timer = rospy.Timer(rospy.Duration(1), self.policy_loop)
#taurob_training_week = rospy.get_param('/taurob_training_week',False)
# connect Signal Slot
#self._update_task_delegates.connect(self._on_update_task_delegates)
# init stuff
# self.bla = 1
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 _on_joint_states(self,message):
# arm_joints =['arm_joint_0', 'arm_joint_1', 'arm_joint_2', 'arm_joint_3', 'arm_joint_4']
############################################################
######################### BUTTONS ##########################
############################################################
###################################
# Setup button (Set Goal to Cars) #
###################################
def _on_setup_button_pressed(self):
# should send two navigation goals
print(' Setup Button pressed, publishing msg')
# goal for first car
goal1 = MoveBaseGoal()
goal1.target_pose.header.frame_id = "/car1/map"
goal1.target_pose.header.stamp = rospy.Time.now()
goal1.target_pose.pose.position.x = self.car1_goal[0]
goal1.target_pose.pose.position.y = self.car1_goal[1]
goal1.target_pose.pose.orientation.z = 1.0
self.goal_client1.send_goal(goal1)
# goal for second car
goal2 = MoveBaseGoal()
goal2.target_pose.header.frame_id = "/car2/map"
goal2.target_pose.header.stamp = rospy.Time.now()
goal2.target_pose.pose.position.x = self.car2_goal[0]
goal2.target_pose.pose.position.y = self.car2_goal[1]
goal2.target_pose.pose.orientation.z = 1.0
self.goal_client2.send_goal(goal2)
time_now = rospy.Time(0)
(trans1, rot1) = self.listener.lookupTransform('/car1/base_link', '/map', time_now)
(trans2, rot2) = self.listener.lookupTransform('/car2/base_link', '/map', time_now)
self.pos_car1 = trans1
self.pos_car2 = trans2
print("car 1: ")
print(self.pos_car1)
print("car 2: ")
print(self.pos_car2)
#####################################################
# Compute policy button (action/observarion/reward) #
#####################################################
def _on_start_button_pressed(self):
# should call compute policy method and compute policy will give list of actions.
# Should execute one action after another (kinda loop). Before or together send
# velocity to another car
# self.compute_policy()
self.execute_policy = True
self.reward_total = 0
self.t = 0
req = ActionObservationRequest()
req.action = 5
res = self.velocity_service2_.call(req)
#print(' Start Button pressed, publishing msg')
########################
# Trajectory Recording #
########################
def _on_record_button_pressed(self):
# Should record trajectory
print('Recording trajectory')
# position = []
self.record_button = True
self.save_button = False
self.trajectory_collector = []
self.timer_position = rospy.Timer(rospy.Duration(1.0 / 30.0), self.trajectory_recording)
#################################
# Trajectory Saving to the File #
#################################
def _on_save_button_pressed(self):
# Should record trajectory
self.record_button = False
self.save_button = True
print('Saving data to the file...')
path = "/home/linjuk/adda_ros/src/code_lina/trajectories"
if not os.path.exists(path):
os.makedirs(path)
self.save_positions(path)
self.save_button = False
self.trajectory_collector = []
# self.seq = self.trajectory_collector
# self.file = open(os.path.join(path, "linaFile.txt"), "w")
# self.file.write(self.trajectory_collector)
#######################
# Plotting Trajectory #
#######################
def _on_plot_button_pressed(self):
# Should plot trajectories
self.plot_button = True
print('Plotting Trajectories...')
print('straight')
# straight_trajectory_files = [
# '/home/linjuk/adda_ros/src/code_lina/trajectories/forward_1.csv',
# '/home/linjuk/adda_ros/src/code_lina/trajectories/forward_2.csv',
# '/home/linjuk/adda_ros/src/code_lina/trajectories/forward_3.csv',
# '/home/linjuk/adda_ros/src/code_lina/trajectories/forward_4.csv',
# '/home/linjuk/adda_ros/src/code_lina/trajectories/forward_5.csv',
# ]
#
# straight_master = []
# for file in straight_trajectory_files:
# print(len(self.calculate_time_step_average(file)))
# straight_master.append(self.calculate_time_step_average(file))
#
#
# straight_master = pd.DataFrame[straight_master]
# print(straight_master)
# straight_master.to_csv('/home/linjuk/adda_ros/src/code_lina/trajectories/100_time_steps_{time}.csv'.format(time = abs(time.time())), sep=',', encoding='utf-8')
print(self.calculate_time_step_average('/home/linjuk/adda_ros/src/code_lina/trajectories/forward_1.csv'))
print(self.calculate_time_step_average('/home/linjuk/adda_ros/src/code_lina/trajectories/forward_2.csv'))
print(self.calculate_time_step_average('/home/linjuk/adda_ros/src/code_lina/trajectories/forward_3.csv'))
print(self.calculate_time_step_average('/home/linjuk/adda_ros/src/code_lina/trajectories/forward_4.csv'))
print(self.calculate_time_step_average('/home/linjuk/adda_ros/src/code_lina/trajectories/forward_5.csv'))
print('left')
print(self.calculate_time_step_average('/home/linjuk/adda_ros/src/code_lina/trajectories/left_parking_front_1.csv'))
print(self.calculate_time_step_average('/home/linjuk/adda_ros/src/code_lina/trajectories/left_parking_front_2.csv'))
print(self.calculate_time_step_average('/home/linjuk/adda_ros/src/code_lina/trajectories/left_parking_front_3.csv'))
print(self.calculate_time_step_average('/home/linjuk/adda_ros/src/code_lina/trajectories/left_parking_front_4.csv'))
print(self.calculate_time_step_average('/home/linjuk/adda_ros/src/code_lina/trajectories/left_parking_front_5.csv'))
print('right')
print(self.calculate_time_step_average('/home/linjuk/adda_ros/src/code_lina/trajectories/right_parking_front_1.csv'))
print(self.calculate_time_step_average('/home/linjuk/adda_ros/src/code_lina/trajectories/right_parking_front_2.csv'))
print(self.calculate_time_step_average('/home/linjuk/adda_ros/src/code_lina/trajectories/right_parking_front_3.csv'))
print(self.calculate_time_step_average('/home/linjuk/adda_ros/src/code_lina/trajectories/right_parking_front_4.csv'))
print(self.calculate_time_step_average('/home/linjuk/adda_ros/src/code_lina/trajectories/right_parking_front_5.csv'))
file_path = '/home/linjuk/adda_ros/src/code_lina/trajectories/left_parking_back_4.csv'
data = []
with open(file_path, 'r') as f:
reader = csv.reader(f, delimiter=',')
# skip header (time, x, y, etc.)
next(reader)
for row in reader:
element = []
for entry in row:
element.append(float(entry))
data.append(element)
return np.asarray(data)
############ 0 PLOT ##############
# numline = len(file_read.readlines())
# row_count = 0
# with open('/home/linjuk/adda_ros/src/code_lina/trajectories/forward_1.csv', 'r') as f:
# reader = csv.reader(f, delimiter=',')
# row_count = sum(1 for row in reader)
# row_count -= 1
#
# with open('/home/linjuk/adda_ros/src/code_lina/trajectories/forward_1.csv', 'r') as f:
# reader = csv.reader(f, delimiter=',')
# required_rows = 100
# bucket = row_count / required_rows
#
# print('Row count')
# print(row_count)
# print('Bucket')
# print(bucket)
#
# # next(reader)
#
# counter = 1
# values_of_interest = 0
# averaged_results = []
# for row in reader:
# if counter % bucket == 0:
# values_of_interest += 1
# list(reader)[counter]
# counter+=1
# print('Values we are interested in')
# print(values_of_interest)
def calculate_time_step_average(self, file):
df = pd.read_csv(file)
# print(df)
row_count = len(df)
required_rows = 100
bucket = int(round(row_count / required_rows))
counter = 0
averaged_x = []
averaged_y = []
averaged_z = []
# aggregate = []
if bucket >= 2:
for index, row in df.iterrows():
if(index % bucket == 0):
averaged_x.append(df[counter:counter+bucket]['x'].mean())
averaged_y.append(df[counter:counter + bucket]['y'].mean())
averaged_z.append(df[counter:counter + bucket]['z'].mean())
# aggregate.append([df[counter:counter + bucket]['x'].mean(),df[counter:counter + bucket]['y'].mean(),df[counter:counter + bucket]['z'].mean()])
counter += bucket
# averaged_x = averaged_x[-100:]
# averaged_y = averaged_y[-100:]
# averaged_z = averaged_z[-100:]
else:
averaged_x = df['x']
averaged_y = df['y']
averaged_z = df['z']
# aggregate = df
return averaged_x, averaged_y, averaged_z
# return len(averaged_x), len(averaged_y), len(averaged_z)
# averaged_xr.to_csv('/home/linjuk/adda_ros/src/code_lina/trajectories/100_time_steps_{time}.csv'.format(time = abs(time.time())), sep=',', encoding='utf-8')
# path = "/home/linjuk/adda_ros/src/code_lina/trajectories/100"
# file_to_save = df.to_csv("100_time_steps.csv".format(path, num("%d", "w")))
############ 1st PLOT ##############
# files = [
# '/home/linjuk/adda_ros/src/code_lina/trajectories/left_parking_back_1.csv',
# '/home/linjuk/adda_ros/src/code_lina/trajectories/left_parking_back_2.csv',
# '/home/linjuk/adda_ros/src/code_lina/trajectories/left_parking_back_3.csv',
# '/home/linjuk/adda_ros/src/code_lina/trajectories/left_parking_back_4.csv',
# '/home/linjuk/adda_ros/src/code_lina/trajectories/left_parking_back_5.csv',
# '/home/linjuk/adda_ros/src/code_lina/trajectories/left_parking_front_1.csv',
# '/home/linjuk/adda_ros/src/code_lina/trajectories/left_parking_front_2.csv',
# '/home/linjuk/adda_ros/src/code_lina/trajectories/left_parking_front_3.csv',
# '/home/linjuk/adda_ros/src/code_lina/trajectories/left_parking_front_4.csv',
# '/home/linjuk/adda_ros/src/code_lina/trajectories/left_parking_front_5.csv',
#
# '/home/linjuk/adda_ros/src/code_lina/trajectories/right_parking_back_1.csv',
# '/home/linjuk/adda_ros/src/code_lina/trajectories/right_parking_back_2.csv',
# '/home/linjuk/adda_ros/src/code_lina/trajectories/right_parking_back_3.csv',
# '/home/linjuk/adda_ros/src/code_lina/trajectories/right_parking_back_4.csv',
# '/home/linjuk/adda_ros/src/code_lina/trajectories/right_parking_back_5.csv',
# '/home/linjuk/adda_ros/src/code_lina/trajectories/right_parking_front_1.csv',
# '/home/linjuk/adda_ros/src/code_lina/trajectories/right_parking_front_2.csv',
# '/home/linjuk/adda_ros/src/code_lina/trajectories/right_parking_front_3.csv',
# '/home/linjuk/adda_ros/src/code_lina/trajectories/right_parking_front_4.csv',
# '/home/linjuk/adda_ros/src/code_lina/trajectories/right_parking_front_5.csv',
#
# '/home/linjuk/adda_ros/src/code_lina/trajectories/back_1.csv',
# '/home/linjuk/adda_ros/src/code_lina/trajectories/back_2.csv',
# '/home/linjuk/adda_ros/src/code_lina/trajectories/back_3.csv',
# '/home/linjuk/adda_ros/src/code_lina/trajectories/back_4.csv',
# '/home/linjuk/adda_ros/src/code_lina/trajectories/back_5.csv',
# '/home/linjuk/adda_ros/src/code_lina/trajectories/forward_1.csv',
# '/home/linjuk/adda_ros/src/code_lina/trajectories/forward_2.csv',
# '/home/linjuk/adda_ros/src/code_lina/trajectories/forward_3.csv',
# '/home/linjuk/adda_ros/src/code_lina/trajectories/forward_4.csv',
# '/home/linjuk/adda_ros/src/code_lina/trajectories/forward_5.csv'
# ]
# file dictionary for easy file management and looping
files_dictionary = {
'left': [
'/home/linjuk/adda_ros/src/code_lina/trajectories/left_parking_back_1.csv',
'/home/linjuk/adda_ros/src/code_lina/trajectories/left_parking_back_2.csv',
'/home/linjuk/adda_ros/src/code_lina/trajectories/left_parking_back_3.csv',
'/home/linjuk/adda_ros/src/code_lina/trajectories/left_parking_back_4.csv',
'/home/linjuk/adda_ros/src/code_lina/trajectories/left_parking_back_5.csv',
'/home/linjuk/adda_ros/src/code_lina/trajectories/left_parking_front_1.csv',
'/home/linjuk/adda_ros/src/code_lina/trajectories/left_parking_front_2.csv',
'/home/linjuk/adda_ros/src/code_lina/trajectories/left_parking_front_3.csv',
'/home/linjuk/adda_ros/src/code_lina/trajectories/left_parking_front_4.csv',
'/home/linjuk/adda_ros/src/code_lina/trajectories/left_parking_front_5.csv',
],
'right': [
'/home/linjuk/adda_ros/src/code_lina/trajectories/right_parking_back_1.csv',
'/home/linjuk/adda_ros/src/code_lina/trajectories/right_parking_back_2.csv',
'/home/linjuk/adda_ros/src/code_lina/trajectories/right_parking_back_3.csv',
'/home/linjuk/adda_ros/src/code_lina/trajectories/right_parking_back_4.csv',
'/home/linjuk/adda_ros/src/code_lina/trajectories/right_parking_back_5.csv',
'/home/linjuk/adda_ros/src/code_lina/trajectories/right_parking_front_1.csv',
'/home/linjuk/adda_ros/src/code_lina/trajectories/right_parking_front_2.csv',
'/home/linjuk/adda_ros/src/code_lina/trajectories/right_parking_front_3.csv',
'/home/linjuk/adda_ros/src/code_lina/trajectories/right_parking_front_4.csv',
'/home/linjuk/adda_ros/src/code_lina/trajectories/right_parking_front_5.csv',
],
'straight': [
'/home/linjuk/adda_ros/src/code_lina/trajectories/back_1.csv',
'/home/linjuk/adda_ros/src/code_lina/trajectories/back_2.csv',
'/home/linjuk/adda_ros/src/code_lina/trajectories/back_3.csv',
'/home/linjuk/adda_ros/src/code_lina/trajectories/back_4.csv',
'/home/linjuk/adda_ros/src/code_lina/trajectories/back_5.csv',
'/home/linjuk/adda_ros/src/code_lina/trajectories/forward_1.csv',
'/home/linjuk/adda_ros/src/code_lina/trajectories/forward_2.csv',
'/home/linjuk/adda_ros/src/code_lina/trajectories/forward_3.csv',
'/home/linjuk/adda_ros/src/code_lina/trajectories/forward_4.csv',
'/home/linjuk/adda_ros/src/code_lina/trajectories/forward_5.csv'
],
}
# container dictionary for ploting graphs
trajectory_csv_data = {}
# container dictionary for averaging
trajectory_csv_file_wise_data = {}
# color map for graph
color_map = {
'left': 'green',
'right': 'blue',
'straight': 'magenta'
}
# plot first graph
plt.figure(1)
plt.title('Movement Clasification f = y(x)')
plt.xlabel('x [m]')
plt.ylabel('y [m]')
# labels
plt.plot(0, 0, color='green', label='Left-Side Parking')
plt.plot(0, 0, color='blue', label='Right-Side Parking')
plt.plot(0, 0, color='magenta', label='Straight-Side Parking')
# loop over trajectories i.e. left, right, straight
for key in files_dictionary:
trajectory_csv_file_wise_data[key] = {}
# loop over files in each trajectory
for index, file in enumerate(files_dictionary[key]):
# read file
trajectory_csv_data[key] = (genfromtxt(file, dtype=float, delimiter=",", skip_header=1))
# aggregate data in container for averaging
trajectory_csv_file_wise_data[key][index] = []
trajectory_csv_file_wise_data[key][index].append(trajectory_csv_data[key])
# segregate x and y for plotting
x, y = trajectory_csv_data[key][:, 1], trajectory_csv_data[key][:, 2]
p = plt.plot(x, y, color=color_map[key])
plt.legend(loc='lower right')
plt.show()
row = []
for index, file in enumerate(files_dictionary['left']):
for rows in enumerate(file[0]):
row.append(trajectory_csv_file_wise_data['left'][index][0][rows])
print(row)
# print(np.mean(row1, axis=0))
# data = genfromtxt('/home/linjuk/adda_ros/src/code_lina/trajectories/left_parking_back_1.csv', dtype=float, delimiter=",")
# data2 = genfromtxt('/home/linjuk/adda_ros/src/code_lina/trajectories/left_parking_back_2.csv', dtype=float, delimiter=",")
# data3 = genfromtxt('/home/linjuk/adda_ros/src/code_lina/trajectories/left_parking_back_3.csv', dtype=float, delimiter=",")
# data4 = genfromtxt('/home/linjuk/adda_ros/src/code_lina/trajectories/left_parking_back_4.csv', dtype=float, delimiter=",")
# data5 = genfromtxt('/home/linjuk/adda_ros/src/code_lina/trajectories/left_parking_back_5.csv', dtype=float, delimiter=",")
# data6 = genfromtxt('/home/linjuk/adda_ros/src/code_lina/trajectories/left_parking_front_1.csv', dtype=float, delimiter=",")
# data7 = genfromtxt('/home/linjuk/adda_ros/src/code_lina/trajectories/left_parking_front_2.csv', dtype=float, delimiter=",")
# data8 = genfromtxt('/home/linjuk/adda_ros/src/code_lina/trajectories/left_parking_front_3.csv', dtype=float, delimiter=",")
# data9 = genfromtxt('/home/linjuk/adda_ros/src/code_lina/trajectories/left_parking_front_4.csv', dtype=float, delimiter=",")
# data10 = genfromtxt('/home/linjuk/adda_ros/src/code_lina/trajectories/left_parking_front_5.csv', dtype=float, delimiter=",")
#
#
# data11 = genfromtxt('/home/linjuk/adda_ros/src/code_lina/trajectories/right_parking_back_1.csv', dtype=float, delimiter=",")
# data12 = genfromtxt('/home/linjuk/adda_ros/src/code_lina/trajectories/right_parking_back_2.csv', dtype=float, delimiter=",")
# data13 = genfromtxt('/home/linjuk/adda_ros/src/code_lina/trajectories/right_parking_back_3.csv', dtype=float, delimiter=",")
# data14 = genfromtxt('/home/linjuk/adda_ros/src/code_lina/trajectories/right_parking_back_4.csv', dtype=float, delimiter=",")
# data15 = genfromtxt('/home/linjuk/adda_ros/src/code_lina/trajectories/right_parking_back_5.csv', dtype=float, delimiter=",")
#
# data16 = genfromtxt('/home/linjuk/adda_ros/src/code_lina/trajectories/right_parking_front_1.csv', dtype=float, delimiter=",")
# data17 = genfromtxt('/home/linjuk/adda_ros/src/code_lina/trajectories/right_parking_front_2.csv', dtype=float, delimiter=",")
# data18 = genfromtxt('/home/linjuk/adda_ros/src/code_lina/trajectories/right_parking_front_3.csv', dtype=float, delimiter=",")
# data19 = genfromtxt('/home/linjuk/adda_ros/src/code_lina/trajectories/right_parking_front_4.csv', dtype=float, delimiter=",")
# data20 = genfromtxt('/home/linjuk/adda_ros/src/code_lina/trajectories/right_parking_front_5.csv', dtype=float, delimiter=",")
#
#
# data21 = genfromtxt('/home/linjuk/adda_ros/src/code_lina/trajectories/back_1.csv', dtype=float, delimiter=",")
# data22 = genfromtxt('/home/linjuk/adda_ros/src/code_lina/trajectories/back_2.csv', dtype=float, delimiter=",")
# data23 = genfromtxt('/home/linjuk/adda_ros/src/code_lina/trajectories/back_3.csv', dtype=float, delimiter=",")
# data24 = genfromtxt('/home/linjuk/adda_ros/src/code_lina/trajectories/back_4.csv', dtype=float, delimiter=",")
# data25 = genfromtxt('/home/linjuk/adda_ros/src/code_lina/trajectories/back_5.csv', dtype=float, delimiter=",")
#
# data26 = genfromtxt('/home/linjuk/adda_ros/src/code_lina/trajectories/forward_1.csv', dtype=float, delimiter=",")
# data27 = genfromtxt('/home/linjuk/adda_ros/src/code_lina/trajectories/forward_2.csv', dtype=float, delimiter=",")
# data28 = genfromtxt('/home/linjuk/adda_ros/src/code_lina/trajectories/forward_3.csv', dtype=float, delimiter=",")
# data29 = genfromtxt('/home/linjuk/adda_ros/src/code_lina/trajectories/forward_4.csv', dtype=float, delimiter=",")
# data30 = genfromtxt('/home/linjuk/adda_ros/src/code_lina/trajectories/forward_5.csv', dtype=float, delimiter=",")
#
# x = data[:, 1]
# x2 = data2[:, 1]
# x3 = data3[:, 1]
# x4 = data4[:, 1]
# x5 = data5[:, 1]
# x6 = data6[:, 1]
# x7 = data7[:, 1]
# x8 = data8[:, 1]
# x9 = data9[:, 1]
# x10 = data10[:, 1]
# x11 = data11[:, 1]
# x12 = data12[:, 1]
# x13 = data13[:, 1]
# x14 = data14[:, 1]
# x15 = data15[:, 1]
# x16 = data16[:, 1]
# x17 = data17[:, 1]
# x18 = data18[:, 1]
# x19 = data19[:, 1]
# x20 = data20[:, 1]
# x21 = data21[:, 1]
# x22 = data22[:, 1]
# x23 = data23[:, 1]
# x24 = data24[:, 1]
# x25 = data25[:, 1]
# x26 = data26[:, 1]
# x27 = data27[:, 1]
# x28 = data28[:, 1]
# x29 = data29[:, 1]
# x30 = data30[:, 1]
#
# y = data[:, 2]
# y2 = data2[:, 2]
# y3 = data3[:, 2]
# y4 = data4[:, 2]
# y5 = data5[:, 2]
# y6 = data6[:, 2]
# y7 = data7[:, 2]
# y8 = data8[:, 2]
# y9 = data9[:, 2]
# y10 = data10[:, 2]
# y11 = data11[:, 2]
# y12 = data12[:, 2]
# y13 = data13[:, 2]
# y14 = data14[:, 2]
# y15 = data15[:, 2]
# y16 = data16[:, 2]
# y17 = data17[:, 2]
# y18 = data18[:, 2]
# y19 = data19[:, 2]
# y20 = data20[:, 2]
# y21 = data21[:, 2]
# y22 = data22[:, 2]
# y23 = data23[:, 2]
# y24 = data24[:, 2]
# y25 = data25[:, 2]
# y26 = data26[:, 2]
# y27 = data27[:, 2]
# y28 = data28[:, 2]
# y29 = data29[:, 2]
# y30 = data30[:, 2]
#
# # plot(x, y, color='green', linestyle='dashed', marker='o', markerfacecolor='blue', markersize=12).
#
# plt.figure(2)
# plt.title('Movement Clasification f = y(x)')
# plt.xlabel('x [m]')
# plt.ylabel('y [m]')
# plt.plot(x, y, color='green', label='Left-Side Parking')
# plt.plot(x2, y2, color='green')
# plt.plot(x3, y3, color='green')
# plt.plot(x4, y4, color='green')
# plt.plot(x5, y5, color='green')
# plt.plot(x6, y6, color='green')
# plt.plot(x7, y7, color='green')
# plt.plot(x8, y8, color='green')
# plt.plot(x9, y9, color='green')
# plt.plot(x10, y10, color='green')
#
# plt.plot(x11, y11, color='blue', label='Right-Side Parking')
# plt.plot(x12, y12, color='blue')
# plt.plot(x13, y13, color='blue')
# plt.plot(x14, y14, color='blue')
# plt.plot(x15, y15, color='blue')
# plt.plot(x16, y16, color='blue')
# plt.plot(x17, y17, color='blue')
# plt.plot(x18, y18, color='blue')
# plt.plot(x19, y19, color='blue')
# plt.plot(x20, y20, color='blue')
#
# plt.plot(x21, y21, color='magenta', label='Going Straight')
# plt.plot(x22, y22, color='magenta')
# plt.plot(x23, y23, color='magenta')
# plt.plot(x24, y24, color='magenta')
# plt.plot(x25, y25, color='magenta')
# plt.plot(x26, y26, color='magenta')
# plt.plot(x27, y27, color='magenta')
# plt.plot(x28, y28, color='magenta')
# plt.plot(x29, y29, color='magenta')
# plt.plot(x30, y30, color='magenta')
#
#
# plt.legend(loc='lower right')
# plt.show()
############################################################
###################### END OF BUTTONS ######################
############################################################
############################################################
# helper function to generate random choice based on wieghts
############################################################
def random_action(self):
actions = [1] * 45 + [2] * 30 + [3] * 25
return random.choice(actions)
###############
# state mutator
###############
def update_state(self):
# TODO: at the moment this function uses initial values but it needs to be replaced with realtime values using the new service
# fetch values from service
# update self.pos_car1 and 2
# self.pos_car1 index => 0 = x, 1 = y, 2 = velocity, 3 = target.x, 4 = target.y
time_now = rospy.Time(0)
(trans1, rot1) = self.listener.lookupTransform('/map', '/car1/base_link', time_now)
(trans2, rot2) = self.listener.lookupTransform('/map', '/car2/base_link', time_now)
self.pos_car1 = trans1
self.pos_car2 = trans2
state = np.zeros((10,1))
state[0] = self.pos_car1[0] # car1 x possition
state[1] = self.pos_car1[1] # car1 y possition
# state[2] = self.velocity_service1_.call(req) # car1 velocity NOT SURE ABOUT THIS
# state[3] = goal.target_pose.pose.position.x # car1 goal x NOT SURE ABOUT THIS
# state[4] = goal.target_pose.pose.position.y # car1 goal y NOT SURE ABOUT THIS
state[5] = self.pos_car2[0] # car2 x possition
state[6] = self.pos_car2[1] # car2 y possition
# state[7] = self.velocity_service2_.call(req) # car1 velocity NOT SURE ABOUT THIS
# state[8] = goal.target_pose.pose.position.x # car1 goal x NOT SURE ABOUT THIS
# state[9] = goal.target_pose.pose.position.y # car1 goal y NOT SURE ABOUT THIS
return state
###########################################
# random action logic inside compute policy
###########################################
def compute_action(self, state):
print('Compute action ...')
# TODO: decide the logic to determine the action based on state, right now its random action with weighted options
print('Generating a random action:')
action = self.random_action()
print(action)
return state, action
############################################
# logic for transition inside compute policy
############################################
def transition(self, state, action):
print('Transition ...')
# perform action suggested by compute_action
req = ActionObservationRequest()
req.action = action
res = self.velocity_service1_.call(req)
# wait for 3 seconds
# time.sleep(1.0)
# get new state
new_state = self.update_state()
# observe environment
observation = np.zeros((7, 1))
observation[0] = self.pos_car1[0] # car1 x possition
observation[1] = self.pos_car1[1] # car1 y possition
# observation[2] = self.velocity_service1_.call(req) # car1 velocity NOT SURE ABOUT THIS
observation[3] = self.pos_car2[0] # car2 x possition
observation[4] = self.pos_car2[1] # car2 y possition
# observation[5] = self.velocity_service2_.call(req) # car2 velocity NOT SURE ABOUT THIS
return new_state, observation
############################
# function for reward logic
############################
def reward(self, state, action, observation):
# Negative reward for the action that was made ( for all actions the same negative reward)
# Negative reward, if the car is to close to the other car (pre-defined range)
# Speed reward ???
# Positive reward, if the car is close to its goal (pre-defined range)
print('Computing reward:')
print('Observation.distance: ', observation[6])
print("car 1: ")
print(self.pos_car1)
print("car 2: ")
print(self.pos_car2)
dist_car = math.sqrt( (state[0] - state[5]) ** 2 + (state[1] - state[6]) ** 2)
dist_goal = math.sqrt((state[0] - self.car1_goal[0]) ** 2 + (state[1] - self.car1_goal[1]) ** 2)
print("distance between cars", dist_car)
print('distance between goal and the car1', dist_goal)
if dist_car >= 10:
reward = 5
elif 10 > dist_car >= 5:
reward = 0
elif 5 > dist_car >= 2:
reward = -10
elif 2 > dist_car >= 0:
reward = -100
#print('Step reward: ', reward)
#print('Total reward: ', reward_total)
return reward
# compute action
# return observation + next state
#############################################
# compute policy loop that computes action,
# makes the transition and gives reward until
# goal is achieved
#############################################
def policy_loop(self, event):
if self.execute_policy:
self.t = self.t + 1
print('EXECUTING POLICY ', self.t)
current_state = self.update_state()
state_before_transition, action = self.compute_action(current_state)
state_next_transition, observation = self.transition(state_before_transition, action)
print('Reward total: ', self.reward_total)
iteration_reward = self.reward(state_next_transition, action, observation)
print('Iteration reward total: ', iteration_reward)
self.reward_total += iteration_reward
# current_state = self.update_state()
# if self.pos_car1[0] >= goal_x and self.pos_car1[1] >= goal_y:
dist_goal1 = math.sqrt((self.pos_car1[0] - self.car1_goal[0]) ** 2 + (self.pos_car1[1] - self.car1_goal[1]) ** 2)
dist_goal2 = math.sqrt((self.pos_car2[0] - self.car2_goal[0]) ** 2 + (self.pos_car2[1] - self.car2_goal[1]) ** 2)
print('dist_goal: ', dist_goal1)
if dist_goal1 < 2:
req = ActionObservationRequest()
req.action = 4
res = self.velocity_service1_.call(req)
self.execute_policy=False
print('dist_goa2: ', dist_goal2)
if dist_goal2 < 2:
req = ActionObservationRequest()
req.action = 4
res = self.velocity_service2_.call(req)
self.execute_policy = False
def save_positions(self, path):
with open("{}/Trajectory_{}.csv".format(path, time.strftime("%Y_%m_%d_%H_%M_%S", time.localtime())), "w") as fp:
writer = csv.writer(fp, delimiter=",")
writer.writerow(["time", "x", "y", "z"])
for point in self.trajectory_collector:
writer.writerow([point[0], point[1][0], point[1][1], point[1][2]])
# [ 123123, [1, 2, 3] ]
def lookup_position(self):
try:
translation, rotation = self.listener.lookupTransform("/map", "/car2/base_link", rospy.Time(0))
self.pos_car2 = translation # [1, 2, 3]
self.trajectory_collector.append([time.time(), self.pos_car2])
except (tf.ExtrapolationException, tf.ConnectivityException, tf.LookupException):
pass
def trajectory_recording(self, event):
if self.record_button:
self.lookup_position()
class MyPlugin(Plugin):
sig_sysmsg = Signal(str)
l_location = pyqtSignal(float, float)
r_location = pyqtSignal(float, float)
LOS_optimize_chal=pyqtSignal(str,str)
NLOS_optimize_chal=pyqtSignal(str)
com_timer = QTimer()
rssi_timer = QTimer()
baro_timer = QTimer()
ConnInd_timer = QTimer()
_pid = 0000000
# save_file_processing
def __init__(self, context):
super(MyPlugin, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('MyPlugin')
rp = rospkg.RosPack()
self._lrssivalue=0.0
self._rrssivalue=0.0
self._lcomvalue=0.0
self._rcomvalue=0.0
self._lbaroaltvalue = 0.0
self._rbaroaltvalue = 0.0
self._enable_gps=0
self._LOS_NLOS=1
self.imu_done=4
self.imu_run_stop_value=1
self.initial_heading=500
self.ros_run_stop_value=1
self.run_initial_scan=1
self.initial_scan_state=1
self.localimu=0
self.remoteimu=0
self.camera_run_save_stop_state=1
self.initial_map=1
self.rssi_test_gps_rssi=1
# 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
## def goCoords():
## def resetError():
## # coordsEdit.setStyleSheet('')
##
## try:
## print("work")
## #latitude, longitude = coordsEdit.text().split(",")
##
## except ValueError:
## #coordsEdit.setStyleSheet("color: red;")
## QTimer.singleShot(500, resetError)
## else:
## map.centerAt(latitude, longitude)
## # map.moveMarker("MyDragableMark", latitude, longitude)
def onMarkerMoved(key, latitude, longitude):
print("Moved!!", key, latitude, longitude)
if key == 'local GPS':
l_coordsEdit.setText("{0:.8}, {1:.8}".format(latitude, longitude))
else:
r_coordsEdit.setText("{0:.8}, {1:.8}".format(latitude, longitude))
#coordsEdit.setText("{}, {}".format(latitude, longitude))
#self.map.moveMarker("local GPS",latitude+0.01, longitude+0.01)
def onMarkerRClick(key):
print("RClick on ", key)
# map.setMarkerOptions(key, draggable=False)
def onMarkerLClick(key):
print("LClick on ", key)
def onMarkerDClick(key):
print("DClick on ", key)
# map.setMarkerOptions(key, draggable=True)
def onMapMoved(latitude, longitude):
print("Moved to ", latitude, longitude)
def onMapRClick(latitude, longitude):
print("RClick on ", latitude, longitude)
def onMapLClick(latitude, longitude):
print("LClick on ", latitude, longitude)
def onMapDClick(latitude, longitude):
print("DClick on ", latitude, longitude)
def move_l_mark(lat_l,lon_l):
#onMarkerMoved("local gps",lat_,lon_)
#print("get location",lat_,lon_)
self.map.moveMarker("local GPS",lat_l,lon_l)
l_coordsEdit.setText("{0:.9}, {1:.9}".format(lat_l,lon_l))
if self.initial_map==1:
self.map.centerAt(lat_l,lon_l)
self.map.setZoom(15)
self.initial_map=0
#time.sleep(0.01)
def local_callback(llocation):
llat=llocation.data[0]
llon=llocation.data[1]
#move_mark(self,lat,lon)
self.l_location.emit(llat,llon)
def move_r_mark(lat_r,lon_r):
#onMarkerMoved("local gps",lat_,lon_)
#print("get location",lat_,lon_)
self.map.moveMarker("remote GPS",lat_r,lon_r)
r_coordsEdit.setText("{0:.9}, {1:.9}".format(lat_r,lon_r))
#time.sleep(0.01)
def remote_callback(rlocation):
rlat=rlocation.data[0]
rlon=rlocation.data[1]
#move_mark(self,lat,lon)
self.r_location.emit(rlat,rlon)
@pyqtSlot()
def change_los_nos_value():
if self._LOS_NLOS==1:
self._LOS_NLOS=0
initial_heading_calculation_button.setText("Run")
else:
self._LOS_NLOS=1
initial_heading_calculation_button.setText("Scan")
print(self._LOS_NLOS)
def optimize_channel_fun():
os.system(
"sudo /home/dnc2/Documents/catkin_ws/src/directional_antenna_system/src/directional_antenna_system/GetRSSI.sh")
if self._LOS_NLOS == 1:
a = localchangechannel.local_change_channel()
print("channel a", a)
time.sleep(2)
b = remotechangechannel.remote_change_channel()
print("channel b", b)
self.LOS_optimize_chal.emit(a,b)
# channel_textedit.setText("L:%s, R:%s" % (a, b))
else:
a = localchangechannel.local_change_channel()
self.NLOS_optimize_chal.emit(a)
# channel_textedit.setText("L:%s, R:N" % a)
@pyqtSlot()
def LOS_channel_testedit(vala,valb):
channel_textedit.setText("L:%s, R:%s" % (vala, valb))
@pyqtSlot()
def NLOS_channel_testedit(vala):
channel_textedit.setText("L:%s, R:N" % vala)
@pyqtSlot()
def channel_on_click():
optimize_channel_process= threading.Thread(target=optimize_channel_fun)
optimize_channel_process.start()
# optimize_channel_process.start()
# imu calibration *******************************************
def run_calibration_fun():
print("calibration")
if (self._LOS_NLOS == 1):
# print("calibrate two IMUs")
os.system("/home/dnc2/Documents/catkin_ws/devel/sshnode/stop_IMU_calibration.sh 1")
time.sleep(7)
os.system( "/home/dnc2/Documents/catkin_ws/devel/sshnode/start_IMU_calibration.sh 1")
else:
# print("calibrate local IMU")
os.system("/home/dnc2/Documents/catkin_ws/devel/sshnode/stop_IMU_calibration.sh 0")
time.sleep(7)
os.system( "/home/dnc2/Documents/catkin_ws/devel/sshnode/start_IMU_calibration.sh 0")
@pyqtSlot()
def imu_run_stop_click():
# imu_threading = threading.Thread(target=run_calibration_fun)
imu_threading= multiprocessing.Process(target=run_calibration_fun)
if self.imu_run_stop_value==1:
print("stat threading")
IMU_run_stop_button.setText("Stop")
IMU_state_state_label.setText("Working")
time.sleep(1)
self.imu_state_timer.start(60000)
imu_threading.start()
self.imu_run_stop_value = 0
else:
IMU_run_stop_button.setText("Run")
self.imu_run_stop_value = 1
IMU_state_state_label.setText("State")
os.system("/home/dnc2/Documents/catkin_ws/devel/sshnode/stop_IMU_calibration.sh 1")
# if imu_threading.isAlive():
# self.imu_threading = threading.Thread(target=run_calibration_fun(selqf, self._LOS_NLOS))
# self.imu_threading.start()
# parent_conn, child_conn = multiprocessing.Pipe()
# self.imu_processing = multiprocessing.Process(target=run_calibration_fun)
# self.imu_processing.start()
def stat_ros_nodes():
print("stat ROS node ...")
# os.system("/home/dnc2/Documents/catkin_ws/devel/sshnode/save_rosbag.sh")
# if self._LOS_NLOS==1:
if self.initial_heading==500:
os.system("/home/dnc2/Documents/catkin_ws/devel/sshnode/stop_ROS_nodes.sh %d" % (self._LOS_NLOS))
time.sleep(7)
os.system("/home/dnc2/Documents/catkin_ws/devel/sshnode/start_ros_no_initial.sh %d " % (self._LOS_NLOS))
else:
os.system("/home/dnc2/Documents/catkin_ws/devel/sshnode/stop_ROS_nodes.sh %d" % (self._LOS_NLOS))
time.sleep(7)
os.system("/home/dnc2/Documents/catkin_ws/devel/sshnode/start_ros_with_initial.sh %d %d" %(self._LOS_NLOS,self.initial_heading))
# else:
# os.system("/home/dnc2/Documents/catkin_ws/devel/sshnode/start_local.sh %d %f" % self._LOS_NLOS,self.initial_heading)
@pyqtSlot()
def stat_ros_click():
# self.imu_timer.stop();
start_ros_node_threading = threading.Thread(target=stat_ros_nodes)
# if not self.monitor_ccq.isRunning():
# self.monitor_ccq.start()
# start_save_threading = threading.Thread(target=start_save_data)
if self.ros_run_stop_value==1:
self.ros_run_stop_value=0
Ros_node_run_button.setText("Stop")
Ros_node_state_label.setText("Working")
IMU_run_stop_button.setEnabled(False)
self.distance_timer.start(10000)
IMU_run_stop_button.setStyleSheet(
"background-color: rgb(192,197,197);padding: 6px; border-radius:5px;text_align:left")
start_ros_node_threading.start()
else:
self.ros_run_stop_value=1
# self.monitor_ccq.terminate()
Ros_node_run_button.setText("Run")
Ros_node_state_label.setText("Done")
IMU_run_stop_button.setEnabled(True)
self.distance_timer.stop()
IMU_run_stop_button.setStyleSheet(
"background-color: rgb(245,128,38);padding: 6px; border-radius:5px;text_align:left")
os.system("/home/dnc2/Documents/catkin_ws/devel/sshnode/stop_ROS_nodes.sh %d" % (self._LOS_NLOS))
# if start_ros_node_threading.isAlive():
# start_save_threading.start()
def run_camera_nodes():
if self.camera_run_save_stop_state==1:
if infrared_camera.isChecked():
camera_comman=10
elif not infrared_camera.isChecked():
camera_comman=0
if optical_camera.isChecked():
opt_camera_comman=10
elif not optical_camera.isChecked():
opt_camera_comman=0
camera_run_button.setText("Confirm")
self.camera_run_save_stop_state = 2
os.system("/home/dnc2/Documents/catkin_ws/devel/sshnode/start_camera_nodes.sh %d %d" %(camera_comman,opt_camera_comman))
elif self.camera_run_save_stop_state==2:
if save_infrared.isChecked():
save_inf_var=1
else:
save_inf_var=0
if save_optical.isChecked():
save_opt_var=1
else:
save_opt_var=0
camera_run_button.setText("Stop")
self.camera_run_save_stop_state = 3
os.system("/home/dnc2/Documents/catkin_ws/devel/sshnode/save_camera_nodes.sh %d %d" % (save_inf_var, save_opt_var))
elif self.camera_run_save_stop_state==3:
camera_run_button.setText("Confirm")
self.camera_run_save_stop_state = 1
os.system("/home/dnc2/Documents/catkin_ws/devel/sshnode/stop_camera_nodes.sh")
# if optical_camera.isChecked() and infrared_camera.isChecked():
# os.system("/home/dnc2/Documents/catkin_ws/devel/sshnode/start_camera_nodes.sh 1 1")
# elif optical_camera.isChecked() and infrared_camera.isChecked()==0:
# os.system("/home/dnc2/Documents/catkin_ws/devel/sshnode/start_camera_nodes.sh 1 0")
# elif optical_camera.isChecked()==0 and infrared_camera.isChecked()==1:
# os.system("/home/dnc2/Documents/catkin_ws/devel/sshnode/start_camera_nodes.sh 0 1")
@pyqtSlot()
def start_camera_click():
start_camera_threading=threading.Thread(target=run_camera_nodes)
if optical_camera.isChecked() or infrared_camera.isChecked():
start_camera_threading.start()
def initial_scan_run():
os.system("/home/dnc2/Documents/catkin_ws/devel/sshnode/start_initial_scan.sh")
@pyqtSlot()
def calculate_Azimuth():
if self._LOS_NLOS==1 and self.run_initial_scan==1:
initial_heading_calculation_button.setText("Stop")
self.run_initial_scan = 0
IMU_run_stop_button.setEnabled(False)
IMU_run_stop_button.setStyleSheet("background-color: rgb(192,197,197);padding: 6px; border-radius:5px;text_align:left")
initial_scan_threading =threading.Thread(target=initial_scan_run)
initial_scan_threading.start()
elif self._LOS_NLOS ==1 and self.run_initial_scan==0:
initial_heading_calculation_button.setText("Run")
self.run_initial_scan = 1
IMU_run_stop_button.setEnabled(True)
IMU_run_stop_button.setStyleSheet(
"background-color: rgb(245,128,38);padding: 6px; border-radius:5px;text_align:left")
# os.system("/home/dnc2/Documents/catkin_ws/devel/sshnode/stop_initial_scan.sh")
if self._LOS_NLOS==0:
if l_coordsEdit.text() and l_coordsEdit.text().find(",") !=-1:
llocation_string = l_coordsEdit.text()
print(llocation_string)
l_lat_lon = llocation_string.split(",")
try:
l_latitiude = float(l_lat_lon[0])
l_longitude = float(l_lat_lon[1])
except:
Azimuth_warning()
return
if r_coordsEdit.text() and r_coordsEdit.text().find(",") != -1:
rlocation_string = r_coordsEdit.text()
print(rlocation_string)
r_lat_lon = rlocation_string.split(",")
try:
r_latitiude = float(r_lat_lon[0])
r_longitude = float(r_lat_lon[1])
temp_remote_gps_msg.data= [r_latitiude, r_longitude]
temp_remote_gps.publish(temp_remote_gps_msg)
# print("temp_gps_test1")
# temp_remote_gps.publish(temp_remote_gps_msg)
# print("temp_gps_test")
except:
Azimuth_warning()
return
try:
LON1 = l_longitude*0.0174532925199433
LAT1 = l_latitiude*0.0174532925199433
LON2 = r_longitude*0.0174532925199433
LAT2 = r_latitiude*0.0174532925199433
az = math.atan2(math.sin(LON2-LON1) * math.cos(LAT2),math.cos(LAT1) * math.sin(LAT2) - math.sin(LAT1) * math.cos(LAT2) * math.cos(LON2-LON1))
az = math.fmod(az, 9.118906528)
az = math.fmod(az, 6.283185307179586)*(57.2957795131)
if az<0 :
az = az+360
Azimuth =az
except:
Azimuth_warning()
return
initial_heading_display_label.setText(" %0.1f"%Azimuth)
self.initial_heading=int(Azimuth)
@pyqtSlot()
def calculate_distance():
if l_coordsEdit.text() and l_coordsEdit.text().find(",") != -1:
llocation_string = l_coordsEdit.text()
l_lat_lon = llocation_string.split(",")
try:
l_latitiude = float(l_lat_lon[0])
l_longitude = float(l_lat_lon[1])
except:
# Azimuth_warning()
# print("error1")
return
if r_coordsEdit.text() and r_coordsEdit.text().find(",") != -1:
rlocation_string = r_coordsEdit.text()
r_lat_lon = rlocation_string.split(",")
try:
r_latitiude = float(r_lat_lon[0])
r_longitude = float(r_lat_lon[1])
except:
# Azimuth_warning()
# print("error2")
return
try:
LON1 = l_longitude * 0.0174532925199433
LAT1 = l_latitiude * 0.0174532925199433
LON2 = r_longitude * 0.0174532925199433
LAT2 = r_latitiude * 0.0174532925199433
A =math.sin((LAT2-LAT1)/2)*math.sin((LAT2-LAT1)/2)+math.cos(LAT1)*math.cos(LAT2)*math.sin((LON2-LON1)/2)*math.sin((LON2-LON1)/2)
# print('A',A)
AB= 2*math.atan2(math.sqrt(A),math.sqrt(1-A))
# print('AB',AB)
AB_DIS=6371000*AB
# print('AB_DIS',AB_DIS)
# distance_text.setText("%0.1f m"%AB_DIS)
self.distance_label.setText("%0.1f m"%AB_DIS)
# distance_lable.setText("0.1f m"%AB_DIS)
except:
# print("error3")
# Azimuth_warning()
return
def Azimuth_warning():
msg = QMessageBox()
msg.setIcon(QMessageBox.Warning)
msg.setWindowTitle("Azimuth calculation")
msg.setText("The locations of local and remote UAVs cannot be empty.\nPlease input locations(latitude,longitude) correctly!")
msg.exec_()
# @pyqtSlot()
# def pub_new_Azimuth():
# try:
# N_Azimuth = float(Azimuth_data.text())
# except:
# Azimuth_warning()
# return
# if math.fabs(N_Azimuth)>= 360:
# Azimuth_warning()
# return
# pub_Azimuth.publish(N_Azimuth)
@pyqtSlot()
def change_baroalt():
# self._rrssivalue=str(rssi2)
r_baro_text.setText("%.1f" % self._rbaroaltvalue)
l_baro_text.setText("%.1f" % self._lbaroaltvalue)
time.sleep(0.01)
# time.sleep(0.01)
def l_baro_callback(lbaroinfo):
self._lbaroaltvalue = lbaroinfo.data[2]
def r_baro_callback(rbaroinfo):
self._rbaroaltvalue = rbaroinfo.data[2]
@pyqtSlot()
def change_rssi():
#self._rrssivalue=str(rssi2)
r_rssi_text.setText(str(self._rrssivalue))
l_rssi_text.setText(str(self._lrssivalue))
time.sleep(0.01)
#time.sleep(0.01)
def l_rssi_callback(rssil):
self._lrssivalue=rssil.data
# self.l_rssi.emit(rssil.data)
# del rssil
#l_rssi_text.update()
#l_rssi_text.setText(str(rssi.data))
def r_rssi_callback(rssir):
self._rrssivalue=rssir.data
# self.r_rssi.emit(rssir.data)
# self.memory_tracker.print_diff()
# print(sys.getsizeof(move_l_compass(headidng1)))
#r_rssi_text.setText(str(rssi.data))
@pyqtSlot()
def move_compass():
l_com_text.setText("%.1f" % self._lcomvalue)
time.sleep(0.01)
# print('work')
l_com_widget.setAngle(self._lcomvalue)
time.sleep(0.01)
r_com_text.setText("%.1f" % self._rcomvalue)
time.sleep(0.01)
r_com_widget.setAngle(self._rcomvalue)
time.sleep(0.01)
# def map_rssi_fun_click():
# if self.rssi_test_gps_rssi==1:
# for i in range(5):
# pub_gps_Azimuth_enable.publish(0)
# time.sleep(0.2)
# self.rssi_test_gps_rssi=0
# else:
# for i in range(5):
# pub_gps_Azimuth_enable.publish(1)
# time.sleep(0.2)
# self.rssi_test_gps_rssi = 1
def l_com_callback(heading):
self._lcomvalue=heading.data
# print('work1')
#l_com_text.setText("%.1f" % heading.data)
# self.l_com.emit(heading.data)
# heading.data =None
#self.update()
#l_com_widget.setAngle(heading.data)
def r_com_callback(heading):
self._rcomvalue=heading.data
#r_com_text.setText("%.1f" % heading.data)
# self.r_com.emit(heading.data)
# del heading
#r_com_widget.setAngle(heading.data)
def local_imucal_callback(l_imu_msg):
self.localimu=l_imu_msg.data
# if self.localimu == 1 and self.remoteimu == 1:
# imu_lock.acquire()
# try:
# IMU_state_state_label.setText("Done")
# finally:
# imu_lock.release()
def remote_imucal_callback(r_imu_msg):
self.remoteimu=r_imu_msg.data
# if self.localimu == 1 and self.remoteimu == 1:
# imu_lock.acquire()
# try:
# IMU_state_state_label.setText("Done")
# finally:
# imu_lock.release()
@pyqtSlot()
def imu_state_display():
if self._LOS_NLOS==1:
if self.remoteimu==1 and self.localimu==1:
IMU_state_state_label.setText("Done")
self.imu_state_timer.stop()
else:
if self.localimu == 1:
IMU_state_state_label.setText("Done")
self.imu_state_timer.stop()
@pyqtSlot()
def local_bbb_conn(var):
if var ==1:
lbbb_conn_icon.setPixmap(green_icon)
else:
lbbb_conn_icon.setPixmap(red_icon)
@pyqtSlot()
def remote_bbb_conn(var):
if var==1:
rbbb_conn_icon.setPixmap(green_icon)
else:
rbbb_conn_icon.setPixmap(red_icon)
@pyqtSlot()
def local_connection_indicator(): # var: RSSI
l_var = self._lrssivalue
r_var = self._rrssivalue
#r_var = -55
#l_var = -75
if l_var>=-60 and l_var<0:
l_con_ind_icon.setPixmap(ConnectionBar5)
elif l_var<-60 and l_var>=-70:
l_con_ind_icon.setPixmap(ConnectionBar4)
elif l_var <-70 and l_var>= -80:
l_con_ind_icon.setPixmap(ConnectionBar3)
elif l_var <-80 and l_var > -96:
l_con_ind_icon.setPixmap(ConnectionBar2)
else:
l_con_ind_icon.setPixmap(ConnectionBar1)
if r_var >= -60 and l_var<0:
r_con_ind_icon.setPixmap(ConnectionBar5)
elif r_var < -60 and r_var >= -70:
r_con_ind_icon.setPixmap(ConnectionBar4)
elif r_var < -70 and r_var >= -80:
r_con_ind_icon.setPixmap(ConnectionBar3)
elif r_var < -80 and r_var > -96:
r_con_ind_icon.setPixmap(ConnectionBar2)
else:
r_con_ind_icon.setPixmap(ConnectionBar1)
# def local_connection_indicator(): # var: RSSI
# #var = self._rrssivalue
# var = -55
# if var>=-60:
# l_con_ind_icon.setPixmap(ConnectionBar5)
# elif var<-60 and var>=-70:
# l_con_ind_icon.setPixmap(ConnectionBar4)
# elif var <-70 and var>= -80:
# l_con_ind_icon.setPixmap(ConnectionBar3)
# elif var <-80 and var > -96:
# l_con_ind_icon.setPixmap(ConnectionBar2)
# else:
# l_con_ind_icon.setPixmap(ConnectionBar1)
self._widget = QWidget()
gcv=QVBoxLayout(self._widget)
v = QVBoxLayout()
h = QHBoxLayout()
FUN_FONT= QFont()
FUN_FONT.setBold(True)
FUN_FONT.setPointSize(14)
FUN_FONT.setWeight(87)
pub_gps_Azimuth_enable = rospy.Publisher('lenablegpsazimuth', Int32, queue_size=10)
temp_remote_gps = rospy.Publisher('temp_gps_location', Float64MultiArray,queue_size=1)
temp_remote_gps_msg= Float64MultiArray()
#ros_threading
#############################
# star_ros_threading = threading.Thread(target= stat_ros_nodes)
# # save_file_processing = multiprocessing.Process(target= save_topic_fun)
# save_file_processing = threading.Thread(target= save_topic_fun)
# star_ros_manual_com_threading = threading.Thread(target= stat_ros_manual_com_nodes)
# imu_threading = threading.Thread(target=run_calibration_fun)
green_icon = QPixmap(("/home/dnc2/Documents/catkin_ws/src/directional_antenna_system/src/directional_antenna_system/greenenergy.png"))
green_icon = green_icon.scaled(40, 40, Qt.KeepAspectRatio)
red_icon = QPixmap(("/home/dnc2/Documents/catkin_ws/src/directional_antenna_system/src/directional_antenna_system/redenergy.png"))
red_icon = red_icon.scaled(40, 40, Qt.KeepAspectRatio)
ConnectionBar1 = QPixmap(("/home/dnc2/Documents/catkin_ws/src/directional_antenna_system/src/directional_antenna_system/ConnectionIndex1.png"))
ConnectionBar2 = QPixmap(("/home/dnc2/Documents/catkin_ws/src/directional_antenna_system/src/directional_antenna_system/ConnectionIndex2.png"))
ConnectionBar3 = QPixmap(("/home/dnc2/Documents/catkin_ws/src/directional_antenna_system/src/directional_antenna_system/ConnectionIndex3.png"))
ConnectionBar4 = QPixmap(("/home/dnc2/Documents/catkin_ws/src/directional_antenna_system/src/directional_antenna_system/ConnectionIndex4.png"))
ConnectionBar5 = QPixmap(("/home/dnc2/Documents/catkin_ws/src/directional_antenna_system/src/directional_antenna_system/ConnectionIndex5.png"))
ConnectionBar1 = ConnectionBar1.scaled(50, 110, Qt.KeepAspectRatio)
ConnectionBar2 = ConnectionBar2.scaled(50, 110, Qt.KeepAspectRatio)
ConnectionBar3 = ConnectionBar3.scaled(50, 110, Qt.KeepAspectRatio)
ConnectionBar4 = ConnectionBar4.scaled(50, 110, Qt.KeepAspectRatio)
ConnectionBar5 = ConnectionBar5.scaled(50, 110, Qt.KeepAspectRatio)
##################################
# local widget
local_widget = QWidget()
local_Vlayout = QVBoxLayout(local_widget)
local_widget.setStyleSheet("background-color: white")
l_coordsEdit = QLineEdit()
l_coordsEdit.setFont(FUN_FONT)
l_coordsEdit.setMinimumWidth(230)
lgps_label = QLabel('GPS:')
lgps_label.setFont(FUN_FONT)
l_com_widget = CompassWidget()
l_rssi_lable = QLabel('RSSI:')
l_rssi_lable.setFont(FUN_FONT)
l_rssi_text = QLineEdit()
l_rssi_text.setFont(FUN_FONT)
l_rssi_text.setMaximumWidth(70)
l_baro_lable = QLabel('Alt:')
l_baro_lable.setFont(FUN_FONT)
l_baro_text = QLineEdit()
l_baro_text.setFont(FUN_FONT)
l_baro_text.setMaximumWidth(80)
l_com_label = QLabel('Antenna Heading:')
l_com_label.setFont(FUN_FONT)
l_com_text = QLineEdit()
l_com_text.setFont(FUN_FONT)
# l_com_text.setText(self._lcomvalue)
l_com_text.setMaximumWidth(80)
l_h1 = QHBoxLayout()
l_h1.addStretch()
l_h1.addWidget(l_rssi_lable)
l_h1.addWidget(l_rssi_text)
l_h1.addWidget(l_com_label)
l_h1.addWidget(l_com_text)
l_h1.addStretch()
l_h2 = QHBoxLayout()
l_h2.addStretch()
l_h2.addWidget(lgps_label)
l_h2.addWidget(l_coordsEdit)
l_h2.addWidget(l_baro_lable)
l_h2.addWidget(l_baro_text)
l_h2.addStretch()
# l_h1.setSpacing(0)
local_Vlayout.addWidget(l_com_widget)
local_Vlayout.addLayout(l_h1)
local_Vlayout.addLayout(l_h2)
local_widget.setMinimumSize(450,300)
local_widget.setMaximumWidth(500)
lbbb_conn_icon=QLabel(l_com_widget)
lbbb_conn_icon.setText("ICON")
lbbb_conn_icon.setGeometry(380,200,40,40)
lbbb_conn_icon.setPixmap(red_icon)
l_con_ind_icon = QLabel(l_com_widget)
l_con_ind_icon.setText("ICON")
l_con_ind_icon.setGeometry(380,40,50,110)
l_con_ind_icon.setPixmap(ConnectionBar1)
##################################
# remote widget
remote_widget = QWidget()
remote_vlayout = QVBoxLayout(remote_widget)
remote_widget.setStyleSheet("background-color: white")
r_coordsEdit = QLineEdit()
r_coordsEdit.setFont(FUN_FONT)
r_coordsEdit.setMinimumWidth(230)
rgps_label = QLabel('GPS:')
rgps_label.setFont(FUN_FONT)
r_com_widget = RCompassWidget()
r_rssi_lable=QLabel('RSSI')
r_rssi_lable.setFont(FUN_FONT)
r_rssi_text=QLineEdit()
r_rssi_text.setFont(FUN_FONT)
r_rssi_text.setMaximumWidth(70)
r_baro_lable = QLabel('Alt:')
r_baro_lable.setFont(FUN_FONT)
r_baro_text = QLineEdit()
r_baro_text.setFont(FUN_FONT)
r_baro_text.setMaximumWidth(80)
r_com_label=QLabel('Antenna Heading:')
r_com_label.setFont(FUN_FONT)
r_com_text=QLineEdit()
r_com_text.setFont(FUN_FONT)
r_com_text.setMaximumWidth(80)
r_h1 = QHBoxLayout()
r_h1.addStretch()
r_h1.addWidget(r_rssi_lable)
r_h1.addWidget(r_rssi_text)
r_h1.addWidget(r_com_label)
r_h1.addWidget(r_com_text)
r_h1.addStretch()
r_h2 =QHBoxLayout()
r_h2.addStretch()
r_h2.addWidget(rgps_label)
r_h2.addWidget(r_coordsEdit)
r_h2.addWidget(r_baro_lable)
r_h2.addWidget(r_baro_text)
r_h2.addStretch()
remote_vlayout.addWidget(r_com_widget)
remote_vlayout.addLayout(r_h1)
remote_vlayout.addLayout(r_h2)
remote_widget.setMinimumSize(450,300)
# remote_widget.setMinimumSize(local_widget.width())
rbbb_conn_icon = QLabel(r_com_widget)
rbbb_conn_icon.setText("ICON")
rbbb_conn_icon.setGeometry(380, 200, 40, 40)
rbbb_conn_icon.setPixmap(red_icon)
r_con_ind_icon = QLabel(r_com_widget)
r_con_ind_icon.setText("ICON")
r_con_ind_icon.setGeometry(380, 40, 50, 110)
r_con_ind_icon.setPixmap(ConnectionBar1)
##################################
#distance and CCQ
distance_lable=QLabel("Distance")
distance_text=QLabel("")
CCQ_lable=QLabel("Transmit CCQ: ")
CCQ_text=QLabel()
DIS_CCQ_widget= QWidget()
DIS_CCQ_layout=QHBoxLayout(DIS_CCQ_widget)
DIS_CCQ_layout.addStretch(1)
DIS_CCQ_layout.addWidget(distance_lable)
DIS_CCQ_layout.addWidget(distance_text)
DIS_CCQ_layout.addStretch(2)
DIS_CCQ_layout.addWidget(CCQ_lable)
DIS_CCQ_layout.addWidget(CCQ_text)
DIS_CCQ_layout.addStretch(1)
DIS_CCQ_widget.setStyleSheet("background-color: white")
# functional buttons
func_widget = QWidget()
Func_widget_layout= QGridLayout(func_widget)
# Func_widget_layout.setRowStretch(0,1)
Func_widget_layout.setHorizontalSpacing(5)
# Func_widget_layout.setSpacing(1)
distan_option_label= QLabel("Distance Option: ")
LOS = QRadioButton("LOS")
LOS.setChecked(True)
NLOS = QRadioButton("NLOS")
NLOS.setChecked(False)
channel_selection_label= QLabel("Channel Selection: ")
channel_textedit= QLineEdit("CH")
channel_optimize= QPushButton("Optimize")
self.LOS_optimize_chal.connect(LOS_channel_testedit)
self.NLOS_optimize_chal.connect(NLOS_channel_testedit)
IMU_calibration_label= QLabel("IMU Calibration: ")
IMU_run_stop_button = QPushButton("Run")
# IMU_run_stop_button.seta
IMU_state_state_label= QLineEdit("state")
initial_heading_label=QLabel("Initial Heading: ")
initial_heading_display_label= QLineEdit("Default")
initial_heading_calculation_button = QPushButton("Scan")
ROS_node_label= QLabel("ROS Nodes: ")
Ros_node_run_button= QPushButton("Run")
Ros_node_state_label= QLineEdit("state")
camera_label =QLabel("Cameras in Use: ")
camera_group_widget=QWidget()
camera_group=QGridLayout(camera_group_widget)
# camera_group.setAlignment(Qt.AlignLeft)
optical_camera=QCheckBox("Optical")
optical_camera.setChecked(True)
infrared_camera=QCheckBox("Infrared")
infrared_camera.setChecked(True)
save_infrared=QCheckBox("Save")
save_optical=QCheckBox("Save")
camera_group.addWidget(optical_camera,0,0)
camera_group.addWidget(save_optical,0,1)
camera_group.addWidget(infrared_camera,1,0)
camera_group.addWidget(save_infrared,1,1)
camera_run_button= QPushButton("Confirm")
#map_rssi_button=QPushButton("RSSI_MAPING")
Func_widget_layout.addWidget(distan_option_label,0,0)
Func_widget_layout.addWidget(LOS,0,1)
Func_widget_layout.addWidget(NLOS,0,2)
Func_widget_layout.addWidget(channel_selection_label,1,0)
Func_widget_layout.addWidget(channel_textedit,1,2)
Func_widget_layout.addWidget(channel_optimize,1,1)
Func_widget_layout.addWidget(IMU_calibration_label,2,0)
Func_widget_layout.addWidget(IMU_run_stop_button,2,1)
Func_widget_layout.addWidget(IMU_state_state_label,2,2)
Func_widget_layout.addWidget(initial_heading_label,3,0)
Func_widget_layout.addWidget(initial_heading_display_label,3,2)
Func_widget_layout.addWidget(initial_heading_calculation_button,3,1)
Func_widget_layout.addWidget(ROS_node_label,4,0)
Func_widget_layout.addWidget(Ros_node_run_button,4,1)
Func_widget_layout.addWidget(Ros_node_state_label,4,2)
Func_widget_layout.addWidget(camera_label, 5,0)
Func_widget_layout.addWidget(camera_group_widget,5,1)
Func_widget_layout.addWidget(camera_run_button,5,2)
#Func_widget_layout.addWidget(map_rssi_button, 6,0)
Func_widget_layout.setColumnMinimumWidth(2,70)
# Func_widget_layout.set
func_widget.setStyleSheet("color:white;font:bold 15px")
# font1 = QFont()
# font1.setPointSize(20)
# # font1.setPixelSize(30)
# # font1.setBold(True)
# font1.setWeight(75)
LOS.setStyleSheet("background-color: rgb(245,128,38);padding: 6px; border-radius:5px")
channel_optimize.setStyleSheet("background-color: rgb(245,128,38);padding: 6px; border-radius:5px")
# channel_textedit.setStyleSheet("background-color: rgb(245,128,38)")
NLOS.setStyleSheet("background-color: rgb(245,128,38);padding: 6px; border-radius:5px")
camera_run_button.setStyleSheet("background-color: rgb(245,128,38);padding: 6px; border-radius:5px")
IMU_run_stop_button.setStyleSheet("background-color: rgb(245,128,38);padding: 6px; border-radius:5px;text_align:left")
# IMU_state_state_label.setStyleSheet("background-color: rgb(245,128,38)")
initial_heading_calculation_button.setStyleSheet("background-color: rgb(245,128,38);padding: 6px; border-radius:5px")
Ros_node_run_button.setStyleSheet("background-color: rgb(245,128,38);padding: 6px; border-radius:5px;text_align:left")
camera_run_button.setStyleSheet("background-color: rgb(245,128,38);padding: 6px; border-radius:5px")
#map_rssi_button.setStyleSheet("background-color: rgb(245,128,38);padding: 6px; border-radius:5px")
# camera_group.setStyleSheet("font:bold 11px")
###############################
LOS.toggled.connect(change_los_nos_value)
channel_optimize.clicked.connect(channel_on_click)
IMU_run_stop_button.clicked.connect(imu_run_stop_click)
initial_heading_calculation_button.clicked.connect(calculate_Azimuth)
Ros_node_run_button.clicked.connect(stat_ros_click)
camera_run_button.clicked.connect(start_camera_click)
#map_rssi_button.clicked.connect(map_rssi_fun_click)
# channel_button.clicked.connect(channel_on_click)
# ROS_button.clicked.connect(stat_ros_click)
# save_button.clicked.connect(save_file_click)
# Azimuth_button.clicked.connect(calculate_Azimuth)
# New_Azimuth_button.clicked.connect(pub_new_Azimuth)
# Enable_gps_Azimuth.clicked.connect(enable_disable_gps_Azimuth)
# manual_com.clicked.connect(stat_ros_manual_com_click)
mid_layout=QHBoxLayout()
#mid_layout.setSpacing(20)
mid_layout.addWidget(local_widget)
mid_layout.addWidget(remote_widget)
top_layout=QVBoxLayout()
# top_layout.addWidget(DIS_CCQ_widget)
top_layout.addLayout(mid_layout)
# top_layout.setStretchFactor(DIS_CCQ_layout,1)
# top_layout.setStretchFactor(mid_layout,9)
com_fun_layout= QHBoxLayout()
com_fun_layout.addLayout(top_layout)
com_fun_layout.addWidget(func_widget)
com_fun_layout.setStretchFactor(mid_layout,5)
com_fun_layout.setStretchFactor(func_widget,1.5)
# mid_layout.addWidget(func_widget)
# mid_layout.setStretchFactor(local_widget,4)
# mid_layout.setStretchFactor(remote_vlayout,4)
# mid_layout.setStretchFactor(func_widget,2)
# mid_layout.addStretch(1)
# gcv.addLayout(mid_layout)
gcv.addLayout(com_fun_layout)
# l.addRow('Coords:', coordsEdit)
# l.addRow('lcoords:',r_coordsEdit)
#h.addLayout(v)
#l = QFormLayout()
#h.addLayout(l)
#coordsEdit = QLineEdit()
#l.addRow('Coords:', coordsEdit)
#coordsEdit.editingFinished.connect(goCoords)
self.map = QOSM(self._widget)
self.map.mapMoved.connect(onMapMoved)
self.map.markerMoved.connect(onMarkerMoved)
self.map.mapClicked.connect(onMapLClick)
self.map.mapDoubleClicked.connect(onMapDClick)
self.map.mapRightClicked.connect(onMapRClick)
self.map.markerClicked.connect(onMarkerLClick)
self.map.markerDoubleClicked.connect(onMarkerDClick)
self.map.markerRightClicked.connect(onMarkerRClick)
self.l_location.connect(move_l_mark)
self.r_location.connect(move_r_mark)
self.distance_widget=QWidget(self.map)
self.distance_widget_layout=QHBoxLayout(self.distance_widget)
self.distance_widget_layout.addWidget(QLabel("Distance: "))
self.distance_label = QLabel()
self.distance_label.setText("the distance")
self.distance_widget_layout.addWidget(self.distance_label)
self.distance_widget.setGeometry(50,10,180,30)
self.distance_widget.setStyleSheet("background-color: white")
# self.distance_label.setGeometry(50,10,120,30)
# self.distance_label.setStyleSheet("background-color: rgb(245,128,38);padding: 6px; border-radius:5px")
# self.distance_label.setGeometry()
# self.distance_dock= QDockWidget()
# self.distance_dock.setWidget(self.distance_label)
# self.distance_dock.setFloating(True)
# self.addDockWidget(Qt.LeftDockWidgetArea,self.distance_dock)
# self._widget.addDockWidget(Qt.LeftDockWidgetArea, self.distance_dock)
gcv.addWidget(self.map)
gcv.setStretchFactor(com_fun_layout, 5)
gcv.setStretchFactor(self.map, 4)
self.map.setSizePolicy(
QSizePolicy.MinimumExpanding,
QSizePolicy.MinimumExpanding)
# l_com_timer = QTimer()
# self.l_com_timer.setSingleShot(True)
self.com_timer.timeout.connect(move_compass)
self.com_timer.start(200)
self.rssi_timer.timeout.connect(change_rssi)
self.rssi_timer.start(1000)
self.baro_timer.timeout.connect(change_baroalt)
self.baro_timer.start(1000)
self.ConnInd_timer.timeout.connect(local_connection_indicator)
self.ConnInd_timer.start(1000)
self.imu_state_timer=QTimer()
self.imu_state_timer.setInterval(2000)
# self.imu_state_timer.interval(2000)
self.imu_state_timer.timeout.connect(imu_state_display)
self.distance_timer = QTimer()
self.distance_timer.setInterval(1000)
self.distance_timer.timeout.connect(calculate_distance)
self.distance_timer.start()
# self.monitor_ccq = CCQ_threading()
self.CONN_state=conn_Test()
self.CONN_state.lBBB_conn.connect(local_bbb_conn)
self.CONN_state.rBBB_conn.connect(remote_bbb_conn)
self.CONN_state.start()
# self.memory_tracker=tracker.SummaryTracker(o)
# 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('gps_com_node_v2'), 'resource', 'MyPlugin.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
self._widget.setStyleSheet("background-color:rgb(0,68,124)")
# Give QObjects reasonable names
self._widget.setObjectName('MyPluginUi')
# 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.map.waitUntilReady()
self.map.centerAt(32.731263, -97.114334)
#self.map.centerAt(38.833005, -77.117415)
self.map.setZoom(12)
# Many icons at: https://sites.google.com/site/gmapsdevelopment/
coords = self.map.center()
self.map.addMarker("local GPS", *coords, **dict(
icon="file:///home/dnc2/Documents/catkin_ws/src/gps_com_node _v2/src/gps_com_node_v2/local_uav.png",
draggable=True,
title="locat GPS marker!"
))
coords = coords[0] , coords[1]
self.map.addMarker("remote GPS", *coords, **dict(
# icon="https://thumb.ibb.co/bvE9FT/remote_uav.png",
icon= "file:///home/dnc2/Documents/catkin_ws/src/gps_com_node _v2/src/gps_com_node_v2/remote_uav.png",
draggable=True,
title="remote GPS marker"
))
sub1 = rospy.Subscriber("/local_gps", Float64MultiArray, local_callback)
sub2 = rospy.Subscriber("/remote_gps", Float64MultiArray, remote_callback)
sub3=rospy.Subscriber("/local_rssi",Int32,l_rssi_callback)
sub4=rospy.Subscriber("/remote_rssi",Int32,r_rssi_callback)
sub5=rospy.Subscriber("/local_com",Float64,l_com_callback)
sub6=rospy.Subscriber("/remote_com",Float64,r_com_callback)
sub7=rospy.Subscriber("/local_imucal_msg", Int32, local_imucal_callback)
sub8=rospy.Subscriber("/remote_imucal_msg", Int32, remote_imucal_callback)
sub9=rospy.Subscriber("/local_baro",Float64MultiArray,l_baro_callback)
sub10=rospy.Subscriber("/remote_baro",Float64MultiArray,r_baro_callback)
#time.sleep(10)
def shutdown_plugin(self):
# TODO unregister all publishers here
# self.save_file_processing.terminate()
# os.system("kill ")
pass
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
class IntegerEditor(EditorWidget):
_update_signal = Signal(int)
def __init__(self, updater, config):
super(IntegerEditor, self).__init__(updater, config)
loadUi(ui_int, self)
# Set ranges
self._min = int(config['min'])
self._max = int(config['max'])
self._min_val_label.setText(str(self._min))
self._max_val_label.setText(str(self._max))
self._slider_horizontal.setRange(self._min, self._max)
# TODO: Fix that the naming of _paramval_lineEdit instance is not
# consistent among Editor's subclasses.
self._paramval_lineEdit.setValidator(QIntValidator(self._min,
self._max, self))
# Initialize to default
self._paramval_lineEdit.setText(str(config['default']))
self._slider_horizontal.setValue(int(config['default']))
# Make slider update text (locally)
self._slider_horizontal.sliderMoved.connect(self._slider_moved)
# Make keyboard input change slider position and update param server
self._paramval_lineEdit.editingFinished.connect(self._text_changed)
# Make slider update param server
# Turning off tracking means this isn't called during a drag
self._slider_horizontal.setTracking(False)
self._slider_horizontal.valueChanged.connect(self._slider_changed)
# Make the param server update selection
self._update_signal.connect(self._update_gui)
# Add special menu items
self.cmenu.addAction(self.tr('Set to Maximum')).triggered.connect(self._set_to_max)
self.cmenu.addAction(self.tr('Set to Minimum')).triggered.connect(self._set_to_min)
def _slider_moved(self):
# This is a "local" edit - only change the text
self._paramval_lineEdit.setText(str(
self._slider_horizontal.sliderPosition()))
def _text_changed(self):
# This is a final change - update param server
# No need to update slider... update_value() will
self._update_paramserver(int(self._paramval_lineEdit.text()))
def _slider_changed(self):
# This is a final change - update param server
# No need to update text... update_value() will
self._update_paramserver(self._slider_horizontal.value())
def update_value(self, value):
super(IntegerEditor, self).update_value(value)
self._update_signal.emit(int(value))
def _update_gui(self, value):
# Block all signals so we don't loop
self._slider_horizontal.blockSignals(True)
# Update the slider value
self._slider_horizontal.setValue(value)
# Make the text match
self._paramval_lineEdit.setText(str(value))
self._slider_horizontal.blockSignals(False)
def _set_to_max(self):
self._update_paramserver(self._max)
def _set_to_min(self):
self._update_paramserver(self._min)
class MonitorChannel(ChannelInterface):
system_diagnostics_signal = Signal(DiagnosticArray, str)
'''
:ivar DiagnosticArray,str system_diagnostics_signal: signal emit system (nmd) diagnostic messages {DiagnosticArray, grpc_url}.
'''
remote_diagnostics_signal = Signal(DiagnosticArray, str)
'''
:ivar DiagnosticArray,str remote_diagnostics_signal: signal emit diagnostic messages from remote system {DiagnosticArray, grpc_url}.
'''
def clear_cache(self, grpc_url=''):
pass
def get_monitor_manager(self, uri='localhost:12321'):
channel = self.get_insecure_channel(uri)
return mstub.MonitorStub(channel), channel
def get_system_diagnostics_threaded(self,
grpc_url='grpc://localhost:12321'):
self._threads.start_thread("gmsdt_%s" % grpc_url,
target=self.get_system_diagnostics,
args=(grpc_url, True))
def get_system_diagnostics(self,
grpc_url='grpc://localhost:12321',
threaded=False):
rospy.logdebug("get system diagnostics from %s" % (grpc_url))
uri, _ = nmdurl.split(grpc_url)
vm, channel = self.get_monitor_manager(uri)
try:
diagnostic_array = vm.get_system_diagnostics()
if threaded:
self.system_diagnostics_signal.emit(diagnostic_array, grpc_url)
self._threads.finished("gmsdt_%s" % grpc_url)
return diagnostic_array
except Exception as e:
self.error.emit("get_system_diagnostics", "grpc://%s" % uri, "", e)
finally:
self.close_channel(channel, uri)
def get_diagnostics_threaded(self, grpc_url='grpc://localhost:12321'):
self._threads.start_thread("gmdt_%s" % grpc_url,
target=self.get_diagnostics,
args=(grpc_url, True))
def get_diagnostics(self,
grpc_url='grpc://localhost:12321',
threaded=False):
rospy.logdebug("get diagnostics from %s" % (grpc_url))
uri, _ = nmdurl.split(grpc_url)
vm, channel = self.get_monitor_manager(uri)
try:
diagnostic_array = vm.get_diagnostics()
if threaded:
self.remote_diagnostics_signal.emit(diagnostic_array, grpc_url)
self._threads.finished("gmdt_%s" % grpc_url)
return diagnostic_array
except Exception as e:
self.error.emit("get_diagnostics", "grpc://%s" % uri, "", e)
finally:
self.close_channel(channel, uri)
def kill_process(self, pid, grpc_url='grpc://localhost:12321'):
rospy.logdebug("kill process %d on %s" % (pid, grpc_url))
uri, _ = nmdurl.split(grpc_url)
vm, channel = self.get_monitor_manager(uri)
try:
vm.kill_process(pid)
except Exception as e:
self.error.emit("kill_process", "grpc://%s" % uri, "", e)
finally:
self.close_channel(channel, uri)
class CapabilityControlWidget(QFrame):
'''
The control widget contains buttons for control a capability. Currently this
are C{On} and C{Off} buttons. Additionally, the state of the capability is
color coded.
'''
start_nodes_signal = Signal(str, str, list)
'''@ivar: the signal is emitted to start on host(described by masteruri) the nodes described in the list, Parameter(masteruri, config, nodes).'''
stop_nodes_signal = Signal(str, list)
'''@ivar: the signal is emitted to stop on masteruri the nodes described in the list.'''
def __init__(self, masteruri, cfg, ns, nodes, parent=None):
QFrame.__init__(self, parent)
self._masteruri = masteruri
self._nodes = {cfg: {ns: nodes}}
frame_layout = QVBoxLayout(self)
frame_layout.setContentsMargins(0, 0, 0, 0)
# create frame for warning label
self.warning_frame = warning_frame = QFrame(self)
warning_layout = QHBoxLayout(warning_frame)
warning_layout.setContentsMargins(0, 0, 0, 0)
warning_layout.addItem(
QSpacerItem(0, 0, QSizePolicy.Expanding, QSizePolicy.Expanding))
self.warning_label = QLabel()
icon = nm.settings().icon('crystal_clear_warning.png')
self.warning_label.setPixmap(icon.pixmap(QSize(40, 40)))
self.warning_label.setToolTip(
'Multiple configuration for same node found!\nA first one will be selected for the start a node!'
)
warning_layout.addWidget(self.warning_label)
warning_layout.addItem(
QSpacerItem(0, 0, QSizePolicy.Expanding, QSizePolicy.Expanding))
frame_layout.addItem(
QSpacerItem(0, 0, QSizePolicy.Expanding, QSizePolicy.Expanding))
frame_layout.addWidget(warning_frame)
# create frame for start/stop buttons
buttons_frame = QFrame()
buttons_layout = QHBoxLayout(buttons_frame)
buttons_layout.setContentsMargins(0, 0, 0, 0)
buttons_layout.addItem(QSpacerItem(20, 20))
self.on_button = QPushButton()
self.on_button.setFlat(False)
self.on_button.setText("On")
self.on_button.clicked.connect(self.on_on_clicked)
buttons_layout.addWidget(self.on_button)
self.off_button = QPushButton()
self.off_button.setFlat(True)
self.off_button.setText("Off")
self.off_button.clicked.connect(self.on_off_clicked)
buttons_layout.addWidget(self.off_button)
buttons_layout.addItem(QSpacerItem(20, 20))
frame_layout.addWidget(buttons_frame)
frame_layout.addItem(
QSpacerItem(0, 0, QSizePolicy.Expanding, QSizePolicy.Expanding))
self.warning_frame.setVisible(False)
def hasConfigs(self):
'''
:return: True, if a configurations for this widget are available.
:rtype: bool
'''
return len(self._nodes) > 0
def nodes(self, cfg=''):
'''
:return: the list with nodes required by this capability. The nodes are
defined by ROS full name.
:rtype: [str]
'''
try:
if cfg:
return [n for l in self._nodes[cfg].values() for n in l]
else:
return [
n for c in self._nodes.values() for l in c.values()
for n in l
]
except Exception:
return []
def setNodeState(self, running_nodes, stopped_nodes, error_nodes):
'''
Sets the state of this capability.
:param running_nodes: a list with running nodes.
:type running_nodes: [str]
:param stopped_nodes: a list with not running nodes.
:type stopped_nodes: [str]
:param error_nodes: a list with nodes having a problem.
:type error_nodes: [str]
'''
self.setAutoFillBackground(True)
self.setBackgroundRole(QPalette.Base)
palette = QPalette()
if error_nodes:
brush = QBrush(QColor(255, 100, 0))
elif running_nodes and stopped_nodes:
brush = QBrush(QColor(140, 185, 255)) # 30, 50, 255
elif running_nodes:
self.on_button.setFlat(True)
self.off_button.setFlat(False)
brush = QBrush(QColor(59, 223, 18)) # 59, 223, 18
else:
brush = QBrush(QColor(255, 255, 255))
self.on_button.setFlat(False)
self.off_button.setFlat(True)
palette.setBrush(QPalette.Active, QPalette.Base, brush)
brush.setStyle(Qt.SolidPattern)
palette.setBrush(QPalette.Inactive, QPalette.Base, brush)
self.setPalette(palette)
def removeCfg(self, cfg):
try:
del self._nodes[cfg]
except Exception:
pass
def updateNodes(self, cfg, ns, nodes):
self._nodes[cfg] = {ns: nodes}
test_nodes = self.nodes()
self.warning_frame.setVisible(len(test_nodes) != len(set(test_nodes)))
def on_on_clicked(self):
started = set() # do not start nodes multiple times
for c in self._nodes.keys():
node2start = set(self.nodes(c)) - started
self.start_nodes_signal.emit(self._masteruri, c, list(node2start))
started.update(node2start)
self.on_button.setFlat(True)
self.off_button.setFlat(False)
def on_off_clicked(self):
self.stop_nodes_signal.emit(self._masteruri, self.nodes())
self.on_button.setFlat(False)
self.off_button.setFlat(True)
class BagTimeline(QGraphicsScene):
"""
BagTimeline contains bag files, all information required to display the bag data visualization
on the screen Also handles events
"""
status_bar_changed_signal = Signal()
selected_region_changed = Signal(rospy.Time, rospy.Time)
def __init__(self, context, publish_clock):
"""
:param context:
plugin context hook to enable adding rqt_bag plugin widgets as ROS_GUI snapin panes,
''PluginContext''
"""
super(BagTimeline, self).__init__()
self._bags = []
self._bag_lock = threading.RLock()
self.background_task = None # Display string
self.background_task_cancel = False
# Playing / Recording
self._playhead_lock = threading.RLock()
self._max_play_speed = 1024.0 # fastest X play speed
self._min_play_speed = 1.0 / 1024.0 # slowest X play speed
self._play_speed = 0.0
self._play_all = False
self._playhead_positions_cvs = {}
self._playhead_positions = {} # topic -> (bag, position)
self._message_loaders = {}
self._messages_cvs = {}
self._messages = {} # topic -> (bag, msg_data)
self._message_listener_threads = {
} # listener -> MessageListenerThread
self._player = False
self._publish_clock = publish_clock
self._recorder = None
self.last_frame = None
self.last_playhead = None
self.desired_playhead = None
self.wrap = True # should the playhead wrap when it reaches the end?
self.stick_to_end = False # should the playhead stick to the end?
self._play_timer = QTimer()
self._play_timer.timeout.connect(self.on_idle)
self._play_timer.setInterval(3)
# Plugin popup management
self._context = context
self.popups = {}
self._views = []
self._listeners = {}
# Initialize scene
# the timeline renderer fixes use of black pens and fills, so ensure we fix white here for
# contrast. Otherwise a dark qt theme will default it to black and the frame render
# pen will be unreadable
self.setBackgroundBrush(Qt.white)
self._timeline_frame = TimelineFrame(self)
self._timeline_frame.setPos(0, 0)
self.addItem(self._timeline_frame)
self.background_progress = 0
self.__closed = False
#----------------------------
def capture(self):
for topic in self._get_topics():
bag, entry = self.get_entry(self._timeline_frame.playhead, topic)
if entry is not None:
topic, msg, t = self.read_message(bag, entry.position)
labels = topic.split('/')
popup_name = labels[1] + '__Capture'
viewer_types = self._timeline_frame.get_viewer_types(
self.get_datatype(topic))
for view_type in viewer_types:
if view_type.name == 'Image' and popup_name not in self.popups:
frame = TopicPopupWidget(popup_name, self, view_type,
str(topic), t)
self.add_view(topic, frame)
self.popups[popup_name] = frame
frame.show(self.get_context())
#-------------------------------
def get_context(self):
"""
:returns: the ROS_GUI context, 'PluginContext'
"""
return self._context
def handle_close(self):
"""
Cleans up the timeline, bag and any threads
"""
if self.__closed:
return
else:
self.__closed = True
self._play_timer.stop()
for topic in self._get_topics():
self.stop_publishing(topic)
self._message_loaders[topic].stop()
if self._player:
self._player.stop()
if self._recorder:
self._recorder.stop()
if self.background_task is not None:
self.background_task_cancel = True
self._timeline_frame.handle_close()
for bag in self._bags:
bag.close()
for frame in self._views:
if frame.parent():
self._context.remove_widget(frame)
# Bag Management and access
def add_bag(self, bag):
"""
creates an indexing thread for each new topic in the bag
fixes the boarders and notifies the indexing thread to index the new items bags
:param bag: ros bag file, ''rosbag.bag''
"""
self._bags.append(bag)
bag_topics = bag_helper.get_topics(bag)
new_topics = set(bag_topics) - set(self._timeline_frame.topics)
for topic in new_topics:
self._playhead_positions_cvs[topic] = threading.Condition()
self._messages_cvs[topic] = threading.Condition()
self._message_loaders[topic] = MessageLoaderThread(self, topic)
self._timeline_frame._start_stamp = self._get_start_stamp()
self._timeline_frame._end_stamp = self._get_end_stamp()
self._timeline_frame.topics = self._get_topics()
self._timeline_frame._topics_by_datatype = self._get_topics_by_datatype(
)
# If this is the first bag, reset the timeline
if self._timeline_frame._stamp_left is None:
self._timeline_frame.reset_timeline()
# Invalidate entire index cache for all topics in this bag
with self._timeline_frame.index_cache_cv:
for topic in bag_topics:
self._timeline_frame.invalidated_caches.add(topic)
if topic in self._timeline_frame.index_cache:
del self._timeline_frame.index_cache[topic]
self._timeline_frame.index_cache_cv.notify()
def file_size(self):
with self._bag_lock:
return sum(b.size for b in self._bags)
# TODO Rethink API and if these need to be visible
def _get_start_stamp(self):
"""
:return: first stamp in the bags, ''rospy.Time''
"""
with self._bag_lock:
start_stamp = None
for bag in self._bags:
bag_start_stamp = bag_helper.get_start_stamp(bag)
if bag_start_stamp is not None and \
(start_stamp is None or bag_start_stamp < start_stamp):
start_stamp = bag_start_stamp
return start_stamp
def _get_end_stamp(self):
"""
:return: last stamp in the bags, ''rospy.Time''
"""
with self._bag_lock:
end_stamp = None
for bag in self._bags:
bag_end_stamp = bag_helper.get_end_stamp(bag)
if bag_end_stamp is not None and (end_stamp is None or
bag_end_stamp > end_stamp):
end_stamp = bag_end_stamp
return end_stamp
def _get_topics(self):
"""
:return: sorted list of topic names, ''list(str)''
"""
with self._bag_lock:
topics = set()
for bag in self._bags:
for topic in bag_helper.get_topics(bag):
topics.add(topic)
return sorted(topics)
def _get_topics_by_datatype(self):
"""
:return: dict of list of topics for each datatype, ''dict(datatype:list(topic))''
"""
with self._bag_lock:
topics_by_datatype = {}
for bag in self._bags:
for datatype, topics in bag_helper.get_topics_by_datatype(
bag).items():
topics_by_datatype.setdefault(datatype, []).extend(topics)
return topics_by_datatype
def get_datatype(self, topic):
"""
:return: datatype associated with a topic, ''str''
:raises: if there are multiple datatypes assigned to a single topic, ''Exception''
"""
with self._bag_lock:
datatype = None
for bag in self._bags:
bag_datatype = bag_helper.get_datatype(bag, topic)
if datatype and bag_datatype and (bag_datatype != datatype):
raise Exception(
'topic %s has multiple datatypes: %s and %s' %
(topic, datatype, bag_datatype))
if bag_datatype:
datatype = bag_datatype
return datatype
def get_entries(self, topics, start_stamp, end_stamp):
"""
generator function for bag entries
:param topics: list of topics to query, ''list(str)''
:param start_stamp: stamp to start at, ''rospy.Time''
:param end_stamp: stamp to end at, ''rospy,Time''
:returns: entries the bag file, ''msg''
"""
with self._bag_lock:
from rosbag import bag # for _mergesort
bag_entries = []
for b in self._bags:
bag_start_time = bag_helper.get_start_stamp(b)
if bag_start_time is not None and bag_start_time > end_stamp:
continue
bag_end_time = bag_helper.get_end_stamp(b)
if bag_end_time is not None and bag_end_time < start_stamp:
continue
connections = list(b._get_connections(topics))
bag_entries.append(
b._get_entries(connections, start_stamp, end_stamp))
for entry, _ in bag._mergesort(bag_entries,
key=lambda entry: entry.time):
yield entry
def get_entries_with_bags(self, topic, start_stamp, end_stamp):
"""
generator function for bag entries
:param topics: list of topics to query, ''list(str)''
:param start_stamp: stamp to start at, ''rospy.Time''
:param end_stamp: stamp to end at, ''rospy,Time''
:returns: tuple of (bag, entry) for the entries in the bag file, ''(rosbag.bag, msg)''
"""
with self._bag_lock:
from rosbag import bag # for _mergesort
bag_entries = []
bag_by_iter = {}
for b in self._bags:
bag_start_time = bag_helper.get_start_stamp(b)
if bag_start_time is not None and bag_start_time > end_stamp:
continue
bag_end_time = bag_helper.get_end_stamp(b)
if bag_end_time is not None and bag_end_time < start_stamp:
continue
connections = list(b._get_connections(topic))
it = iter(b._get_entries(connections, start_stamp, end_stamp))
bag_by_iter[it] = b
bag_entries.append(it)
for entry, it in bag._mergesort(bag_entries,
key=lambda entry: entry.time):
yield bag_by_iter[it], entry
def get_entry(self, t, topic):
"""
Access a bag entry
:param t: time, ''rospy.Time''
:param topic: the topic to be accessed, ''str''
:return: tuple of (bag, entry) corisponding to time t and topic, ''(rosbag.bag, msg)''
"""
with self._bag_lock:
entry_bag, entry = None, None
for bag in self._bags:
bag_entry = bag._get_entry(t, bag._get_connections(topic))
if bag_entry and (not entry or bag_entry.time > entry.time):
entry_bag, entry = bag, bag_entry
return entry_bag, entry
def get_entry_before(self, t):
"""
Access a bag entry
:param t: time, ''rospy.Time''
:return: tuple of (bag, entry) corresponding to time t, ''(rosbag.bag, msg)''
"""
with self._bag_lock:
entry_bag, entry = None, None
for bag in self._bags:
bag_entry = bag._get_entry(t - rospy.Duration(0, 1),
bag._get_connections())
if bag_entry and (not entry or bag_entry.time < entry.time):
entry_bag, entry = bag, bag_entry
return entry_bag, entry
def get_entry_after(self, t):
"""
Access a bag entry
:param t: time, ''rospy.Time''
:return: tuple of (bag, entry) corisponding to time t, ''(rosbag.bag, msg)''
"""
with self._bag_lock:
entry_bag, entry = None, None
for bag in self._bags:
bag_entry = bag._get_entry_after(t, bag._get_connections())
if bag_entry and (not entry or bag_entry.time < entry.time):
entry_bag, entry = bag, bag_entry
return entry_bag, entry
def get_next_message_time(self):
"""
:return: time of the next message after the current playhead position,''rospy.Time''
"""
if self._timeline_frame.playhead is None:
return None
_, entry = self.get_entry_after(self._timeline_frame.playhead)
if entry is None:
return self._timeline_frame._start_stamp
return entry.time
def get_previous_message_time(self):
"""
:return: time of the next message before the current playhead position,''rospy.Time''
"""
if self._timeline_frame.playhead is None:
return None
_, entry = self.get_entry_before(self._timeline_frame.playhead)
if entry is None:
return self._timeline_frame._end_stamp
return entry.time
def resume(self):
if (self._player):
self._player.resume()
# Copy messages to...
def start_background_task(self, background_task):
"""
Verify that a background task is not currently running before starting a new one
:param background_task: name of the background task, ''str''
"""
if self.background_task is not None:
QMessageBox(
QMessageBox.Warning, 'Exclamation',
'Background operation already running:\n\n%s' %
self.background_task, QMessageBox.Ok).exec_()
return False
self.background_task = background_task
self.background_task_cancel = False
return True
def stop_background_task(self):
self.background_task = None
def copy_region_to_bag(self, filename):
if len(self._bags) > 0:
self._export_region(filename, self._timeline_frame.topics,
self._timeline_frame.play_region[0],
self._timeline_frame.play_region[1])
def _export_region(self, path, topics, start_stamp, end_stamp):
"""
Starts a thread to save the current selection to a new bag file
:param path: filesystem path to write to, ''str''
:param topics: topics to write to the file, ''list(str)''
:param start_stamp: start of area to save, ''rospy.Time''
:param end_stamp: end of area to save, ''rospy.Time''
"""
if not self.start_background_task('Copying messages to "%s"' % path):
return
# TODO implement a status bar area with information on the current save status
bag_entries = list(
self.get_entries_with_bags(topics, start_stamp, end_stamp))
if self.background_task_cancel:
return
# Get the total number of messages to copy
total_messages = len(bag_entries)
# If no messages, prompt the user and return
if total_messages == 0:
QMessageBox(QMessageBox.Warning, 'rqt_bag', 'No messages found',
QMessageBox.Ok).exec_()
self.stop_background_task()
return
# Open the path for writing
try:
export_bag = rosbag.Bag(path, 'w')
except Exception:
QMessageBox(QMessageBox.Warning, 'rqt_bag',
'Error opening bag file [%s] for writing' % path,
QMessageBox.Ok).exec_()
self.stop_background_task()
return
# Run copying in a background thread
self._export_thread = threading.Thread(target=self._run_export_region,
args=(export_bag, topics,
start_stamp, end_stamp,
bag_entries))
self._export_thread.start()
def _run_export_region(self, export_bag, topics, start_stamp, end_stamp,
bag_entries):
"""
Threaded function that saves the current selection to a new bag file
:param export_bag: bagfile to write to, ''rosbag.bag''
:param topics: topics to write to the file, ''list(str)''
:param start_stamp: start of area to save, ''rospy.Time''
:param end_stamp: end of area to save, ''rospy.Time''
"""
total_messages = len(bag_entries)
update_step = max(1, total_messages / 100)
message_num = 1
progress = 0
# Write out the messages
for bag, entry in bag_entries:
if self.background_task_cancel:
break
try:
topic, msg, t = self.read_message(bag, entry.position)
export_bag.write(topic, msg, t)
except Exception as ex:
qWarning('Error exporting message at position %s: %s' %
(str(entry.position), str(ex)))
export_bag.close()
self.stop_background_task()
return
if message_num % update_step == 0 or message_num == total_messages:
new_progress = int(100.0 *
(float(message_num) / total_messages))
if new_progress != progress:
progress = new_progress
if not self.background_task_cancel:
self.background_progress = progress
self.status_bar_changed_signal.emit()
message_num += 1
# Close the bag
try:
self.background_progress = 0
self.status_bar_changed_signal.emit()
export_bag.close()
except Exception as ex:
QMessageBox(
QMessageBox.Warning, 'rqt_bag',
'Error closing bag file [%s]: %s' %
(export_bag.filename, str(ex)), QMessageBox.Ok).exec_()
self.stop_background_task()
def read_message(self, bag, position):
with self._bag_lock:
return bag._read_message(position)
# Mouse events
def on_mouse_down(self, event):
if event.buttons() == Qt.LeftButton:
self._timeline_frame.on_left_down(event)
elif event.buttons() == Qt.MidButton:
self._timeline_frame.on_middle_down(event)
elif event.buttons() == Qt.RightButton:
topic = self._timeline_frame.map_y_to_topic(
self.views()[0].mapToScene(event.pos()).y())
TimelinePopupMenu(self, event, topic)
def on_mouse_up(self, event):
self._timeline_frame.on_mouse_up(event)
def on_mouse_move(self, event):
self._timeline_frame.on_mouse_move(event)
def on_mousewheel(self, event):
self._timeline_frame.on_mousewheel(event)
# Zooming
def zoom_in(self):
self._timeline_frame.zoom_in()
def zoom_out(self):
self._timeline_frame.zoom_out()
def reset_zoom(self):
self._timeline_frame.reset_zoom()
def translate_timeline_left(self):
self._timeline_frame.translate_timeline_left()
def translate_timeline_right(self):
self._timeline_frame.translate_timeline_right()
# Publishing
def is_publishing(self, topic):
return self._player and self._player.is_publishing(topic)
def start_publishing(self, topic):
if not self._player and not self._create_player():
return False
self._player.start_publishing(topic)
return True
def stop_publishing(self, topic):
if not self._player:
return False
self._player.stop_publishing(topic)
return True
def _create_player(self):
if not self._player:
try:
self._player = Player(self)
if self._publish_clock:
self._player.start_clock_publishing()
except Exception as ex:
qWarning('Error starting player; aborting publish: %s' %
str(ex))
return False
return True
def set_publishing_state(self, start_publishing):
if start_publishing:
for topic in self._timeline_frame.topics:
if not self.start_publishing(topic):
break
else:
for topic in self._timeline_frame.topics:
self.stop_publishing(topic)
# property: play_all
def _get_play_all(self):
return self._play_all
def _set_play_all(self, play_all):
if play_all == self._play_all:
return
self._play_all = not self._play_all
self.last_frame = None
self.last_playhead = None
self.desired_playhead = None
play_all = property(_get_play_all, _set_play_all)
def toggle_play_all(self):
self.play_all = not self.play_all
# Playing
def on_idle(self):
self._step_playhead()
def _step_playhead(self):
"""
moves the playhead to the next position based on the desired position
"""
# Reset when the playing mode switchs
if self._timeline_frame.playhead != self.last_playhead:
self.last_frame = None
self.last_playhead = None
self.desired_playhead = None
if self._play_all:
self.step_next_message()
else:
self.step_fixed()
def step_fixed(self):
"""
Moves the playhead a fixed distance into the future based on the current play speed
"""
if self.play_speed == 0.0 or not self._timeline_frame.playhead:
self.last_frame = None
self.last_playhead = None
return
now = rospy.Time.from_sec(time.time())
if self.last_frame:
# Get new playhead
if self.stick_to_end:
new_playhead = self.end_stamp
else:
new_playhead = self._timeline_frame.playhead + \
rospy.Duration.from_sec((now - self.last_frame).to_sec() * self.play_speed)
start_stamp, end_stamp = self._timeline_frame.play_region
if new_playhead > end_stamp:
if self.wrap:
if self.play_speed > 0.0:
new_playhead = start_stamp
else:
new_playhead = end_stamp
else:
new_playhead = end_stamp
if self.play_speed > 0.0:
self.stick_to_end = True
elif new_playhead < start_stamp:
if self.wrap:
if self.play_speed < 0.0:
new_playhead = end_stamp
else:
new_playhead = start_stamp
else:
new_playhead = start_stamp
# Update the playhead
self._timeline_frame.playhead = new_playhead
self.last_frame = now
self.last_playhead = self._timeline_frame.playhead
def step_next_message(self):
"""
Move the playhead to the next message
"""
if self.play_speed <= 0.0 or not self._timeline_frame.playhead:
self.last_frame = None
self.last_playhead = None
return
if self.last_frame:
if not self.desired_playhead:
self.desired_playhead = self._timeline_frame.playhead
else:
delta = rospy.Time.from_sec(time.time()) - self.last_frame
if delta > rospy.Duration.from_sec(0.1):
delta = rospy.Duration.from_sec(0.1)
self.desired_playhead += delta
# Get the occurrence of the next message
next_message_time = self.get_next_message_time()
if next_message_time < self.desired_playhead:
self._timeline_frame.playhead = next_message_time
else:
self._timeline_frame.playhead = self.desired_playhead
self.last_frame = rospy.Time.from_sec(time.time())
self.last_playhead = self._timeline_frame.playhead
# Recording
def record_bag(self, filename, all=True, topics=[], regex=False, limit=0):
try:
self._recorder = Recorder(filename,
bag_lock=self._bag_lock,
all=all,
topics=topics,
regex=regex,
limit=limit)
except Exception as ex:
qWarning('Error opening bag for recording [%s]: %s' %
(filename, str(ex)))
return
self._recorder.add_listener(self._message_recorded)
self.add_bag(self._recorder.bag)
self._recorder.start()
self.wrap = False
self._timeline_frame._index_cache_thread.period = 0.1
self.update()
def toggle_recording(self):
if self._recorder:
self._recorder.toggle_paused()
self.update()
def _message_recorded(self, topic, msg, t):
if self._timeline_frame._start_stamp is None:
self._timeline_frame._start_stamp = t
self._timeline_frame._end_stamp = t
self._timeline_frame._playhead = t
elif self._timeline_frame._end_stamp is None or t > self._timeline_frame._end_stamp:
self._timeline_frame._end_stamp = t
if not self._timeline_frame.topics or topic not in self._timeline_frame.topics:
self._timeline_frame.topics = self._get_topics()
self._timeline_frame._topics_by_datatype = self._get_topics_by_datatype(
)
self._playhead_positions_cvs[topic] = threading.Condition()
self._messages_cvs[topic] = threading.Condition()
self._message_loaders[topic] = MessageLoaderThread(self, topic)
# Notify the index caching thread that it has work to do
with self._timeline_frame.index_cache_cv:
self._timeline_frame.invalidated_caches.add(topic)
self._timeline_frame.index_cache_cv.notify()
if topic in self._listeners:
for listener in self._listeners[topic]:
try:
listener.timeline_changed()
except Exception as ex:
qWarning('Error calling timeline_changed on %s: %s' %
(type(listener), str(ex)))
# Dynamically resize the timeline, if necessary, to make visible any new messages
# that might otherwise have exceeded the bounds of the window
self.reset_zoom()
# Views / listeners
def add_view(self, topic, frame):
self._views.append(frame)
def has_listeners(self, topic):
return topic in self._listeners
def add_listener(self, topic, listener):
self._listeners.setdefault(topic, []).append(listener)
self._message_listener_threads[(topic, listener)] = \
MessageListenerThread(self, topic, listener)
# Notify the message listeners
self._message_loaders[topic].reset()
with self._playhead_positions_cvs[topic]:
self._playhead_positions_cvs[topic].notify_all()
self.update()
def remove_listener(self, topic, listener):
topic_listeners = self._listeners.get(topic)
if topic_listeners is not None and listener in topic_listeners:
topic_listeners.remove(listener)
if len(topic_listeners) == 0:
del self._listeners[topic]
# Stop the message listener thread
if (topic, listener) in self._message_listener_threads:
self._message_listener_threads[(topic, listener)].stop()
del self._message_listener_threads[(topic, listener)]
self.update()
# Playhead
# property: play_speed
def _get_play_speed(self):
if self._timeline_frame._paused:
return 0.0
return self._play_speed
def _set_play_speed(self, play_speed):
if play_speed == self._play_speed:
return
if play_speed > 0.0:
self._play_speed = min(self._max_play_speed,
max(self._min_play_speed, play_speed))
elif play_speed < 0.0:
self._play_speed = max(-self._max_play_speed,
min(-self._min_play_speed, play_speed))
else:
self._play_speed = play_speed
if self._play_speed < 1.0:
self.stick_to_end = False
self.update()
play_speed = property(_get_play_speed, _set_play_speed)
def toggle_play(self):
if self._play_speed != 0.0:
self.play_speed = 0.0
else:
self.play_speed = 1.0
def navigate_play(self):
self.play_speed = 1.0
self.last_frame = rospy.Time.from_sec(time.time())
self.last_playhead = self._timeline_frame.playhead
self._play_timer.start()
def navigate_stop(self):
self.play_speed = 0.0
self._play_timer.stop()
def navigate_previous(self):
self.navigate_stop()
self._timeline_frame.playhead = self.get_previous_message_time()
self.last_playhead = self._timeline_frame.playhead
def navigate_next(self):
self.navigate_stop()
self._timeline_frame.playhead = self.get_next_message_time()
self.last_playhead = self._timeline_frame.playhead
def navigate_rewind(self):
if self._play_speed < 0.0:
new_play_speed = self._play_speed * 2.0
elif self._play_speed == 0.0:
new_play_speed = -1.0
else:
new_play_speed = self._play_speed * 0.5
self.play_speed = new_play_speed
def navigate_fastforward(self):
if self._play_speed > 0.0:
new_play_speed = self._play_speed * 2.0
elif self._play_speed == 0.0:
new_play_speed = 2.0
else:
new_play_speed = self._play_speed * 0.5
self.play_speed = new_play_speed
def navigate_start(self):
self._timeline_frame.playhead = self._timeline_frame.play_region[0]
def navigate_end(self):
self._timeline_frame.playhead = self._timeline_frame.play_region[1]
class CapabilityHeader(QHeaderView):
'''
This class is used for visualization of robots or capabilities in header of
the capability table. It is also used to manage the displayed robots or
capabilities. Furthermore U{QtGui.QHeaderView.paintSection()<https://srinikom.github.io/pyside-docs/PySide/QtGui/QHeaderView.html#PySide.QtGui.PySide.QtGui.QHeaderView.paintSection>} method is
overridden to paint the images in background of the cell.
'''
description_requested_signal = Signal(str, str)
'''the signal is emitted by click on a header to show a description.'''
def __init__(self, orientation, parent=None):
QHeaderView.__init__(self, orientation, parent)
self._data = []
'''@ivar: a list with dictionaries C{dict('cfgs': [], 'name': str, 'displayed_name': str, 'type': str, 'description': str, 'images': [QtGui.QPixmap])}'''
if orientation == Qt.Horizontal:
self.setDefaultAlignment(Qt.AlignHCenter | Qt.AlignBottom)
elif orientation == Qt.Vertical:
self.setDefaultAlignment(Qt.AlignLeft | Qt.AlignBottom)
self.controlWidget = []
def index(self, name):
'''
Returns the index of the object stored with given name
:param str name: the name of the item
:return: the index or -1 if the item was not found
:rtype: int
'''
for index, entry in enumerate(self._data):
if entry['name'] == name:
return index
return -1
def paintSection(self, painter, rect, logicalIndex):
'''
The method paint the robot or capability images in the backgroud of the cell.
:see: U{QtGui.QHeaderView.paintSection()<https://srinikom.github.io/pyside-docs/PySide/QtGui/QHeaderView.html#PySide.QtGui.PySide.QtGui.QHeaderView.paintSection>}
'''
painter.save()
QHeaderView.paintSection(self, painter, rect, logicalIndex)
painter.restore()
if logicalIndex in range(len(
self._data)) and self._data[logicalIndex]['images']:
if len(self._data[logicalIndex]['images']) == 1:
pix = self._data[logicalIndex]['images'][0]
pix = pix.scaled(rect.width(),
rect.height() - 20, Qt.KeepAspectRatio,
Qt.SmoothTransformation)
self.style().drawItemPixmap(painter, rect, 5, pix)
elif len(self._data[logicalIndex]['images']) > 1:
new_rect = QRect(rect.left(), rect.top(), rect.width(),
(rect.height() - 20) / 2.)
pix = self._data[logicalIndex]['images'][0]
pix = pix.scaled(new_rect.width(), new_rect.height(),
Qt.KeepAspectRatio, Qt.SmoothTransformation)
self.style().drawItemPixmap(painter, new_rect, 5, pix)
new_rect = QRect(rect.left(),
rect.top() + new_rect.height(), rect.width(),
new_rect.height())
pix = self._data[logicalIndex]['images'][1]
pix = pix.scaled(new_rect.width(), new_rect.height(),
Qt.KeepAspectRatio, Qt.SmoothTransformation)
self.style().drawItemPixmap(painter, new_rect, 5, pix)
def mousePressEvent(self, event):
'''
Interpret the mouse events to send the description of a robot or capability
if the user click on the header.
'''
QHeaderView.mousePressEvent(self, event)
index = self.logicalIndexAt(event.pos())
if index in range(len(self._data)):
suffix = 'Capability'
if self.orientation() == Qt.Horizontal:
suffix = 'Robot'
title = ' - '.join([self._data[index]['name'], suffix])
text = self._data[index]['description']
try:
from docutils import examples
text = examples.html_body(text)
except Exception:
import traceback
rospy.logwarn("Error while generate description for %s: %s",
self._data[index]['name'],
traceback.format_exc(1))
self.description_requested_signal.emit(title, text)
def setDescription(self, index, cfg, name, displayed_name, robot_type,
description, images):
'''
Sets the values of an existing item to the given items.
'''
if index < len(self._data):
obj = self._data[index]
if cfg not in obj['cfgs']:
obj['cfgs'].append(cfg)
obj['name'] = name
if displayed_name:
obj['displayed_name'] = displayed_name
obj['type'] = robot_type
obj['description'] = replace_paths(description)
if images:
del obj['images'][:]
for image_path in images:
img = interpret_path(image_path)
if img and img[0] != os.path.sep:
img = os.path.join(nm.settings().PACKAGE_DIR, image_path)
if os.path.isfile(img):
obj['images'].append(QPixmap(img))
def update_description(self, index, cfg, name, displayed_name, robot_type,
description, images):
'''
Sets the values of an existing item to the given items only if the current
value is empty.
'''
if index < len(self._data):
obj = self._data[index]
if cfg not in obj['cfgs']:
obj['cfgs'].append(cfg)
if not obj['name']:
obj['name'] = name
if not obj['displayed_name']:
obj['displayed_name'] = displayed_name
if not obj['type']:
obj['type'] = robot_type
if not obj['description']:
obj['description'] = replace_paths(description)
if not obj['images']:
for image_path in images:
img = interpret_path(image_path)
if img and img[0] != os.path.sep:
img = os.path.join(nm.settings().PACKAGE_DIR,
image_path)
if os.path.isfile(img):
obj['images'].append(QPixmap(img))
def removeDescription(self, index):
'''
Removes an existing value from the header.
:param str index: the index of the item to remove.
'''
if index < len(self._data):
self._data.pop(index)
def insertItem(self, index):
'''
Inserts an item at the given index into the header.
:param str index: the index
'''
new_dict = {
'cfgs': [],
'name': '',
'displayed_name': '',
'type': '',
'description': '',
'images': []
}
if index < len(self._data):
self._data.insert(index, new_dict)
else:
self._data.append(new_dict)
def insertSortedItem(self, name, displayed_name):
'''
Insert the new item with given name at the sorted position and return the index of
the item.
:param str name: the name of the new item
:return: index of the inserted item
:rtype: int
'''
new_dict = {
'cfgs': [],
'name': name,
'displayed_name': displayed_name,
'type': '',
'description': '',
'images': []
}
for index, item in enumerate(self._data):
if item['displayed_name'].lower() > displayed_name.lower():
self._data.insert(index, new_dict)
return index
self._data.append(new_dict)
return len(self._data) - 1
def removeCfg(self, cfg):
'''
Removes the configuration entries from objects and returns the list with
indexes, where the configuration was removed.
:param str cfg: configuration to remove
:return: the list the indexes, where the configuration was removed
:rtype: [int]
'''
result = []
for index, d in enumerate(self._data):
if cfg in d['cfgs']:
d['cfgs'].remove(cfg)
result.append(index)
return result
def count(self):
'''
:return: The count of items in the header.
:rtype: int
'''
return len(self._data)
def getConfigs(self, index):
'''
:return: The configurations assigned to the item at the given index
:rtype: str
'''
result = []
if index < len(self._data):
result = list(self._data[index]['cfgs'])
return result
class MyPlugin(Plugin):
sig_sysmsg = Signal(str)
l_location = pyqtSignal(float, float)
r_location = pyqtSignal(float, float)
def __init__(self, context):
super(MyPlugin, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('MyPlugin')
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
## def goCoords():
## def resetError():
## # coordsEdit.setStyleSheet('')
##
## try:
## print("work")
## #latitude, longitude = coordsEdit.text().split(",")
##
## except ValueError:
## #coordsEdit.setStyleSheet("color: red;")
## QTimer.singleShot(500, resetError)
## else:
## map.centerAt(latitude, longitude)
## # map.moveMarker("MyDragableMark", latitude, longitude)
def onMarkerMoved(key, latitude, longitude):
print("Moved!!", key, latitude, longitude)
#coordsEdit.setText("{}, {}".format(latitude, longitude))
#self.map.moveMarker("local GPS",latitude+0.01, longitude+0.01)
def onMarkerRClick(key):
print("RClick on ", key)
# map.setMarkerOptions(key, draggable=False)
def onMarkerLClick(key):
print("LClick on ", key)
def onMarkerDClick(key):
print("DClick on ", key)
# map.setMarkerOptions(key, draggable=True)
def onMapMoved(latitude, longitude):
print("Moved to ", latitude, longitude)
def onMapRClick(latitude, longitude):
print("RClick on ", latitude, longitude)
def onMapLClick(latitude, longitude):
print("LClick on ", latitude, longitude)
def onMapDClick(latitude, longitude):
print("DClick on ", latitude, longitude)
def move_l_mark(lat_l, lon_l):
#onMarkerMoved("local gps",lat_,lon_)
#print("get location",lat_,lon_)
self.map.moveMarker("local GPS", lat_l, lon_l)
l_coordsEdit.setText("{0:.8}, {1:.8}".format(lat_l, lon_l))
def local_callback(llocation):
llat = llocation.data[0]
llon = llocation.data[1]
#move_mark(self,lat,lon)
self.l_location.emit(llat, llon)
def move_r_mark(lat_r, lon_r):
#onMarkerMoved("local gps",lat_,lon_)
#print("get location",lat_,lon_)
self.map.moveMarker("remote GPS", lat_r, lon_r)
r_coordsEdit.setText("{0:.8}, {1:.8}".format(lat_r, lon_r))
def remote_callback(rlocation):
rlat = rlocation.data[0]
rlon = rlocation.data[1]
#move_mark(self,lat,lon)
self.r_location.emit(rlat, rlon)
# Create QWidget
self._widget = QWidget()
h = QVBoxLayout(self._widget)
#h = QVBoxLayout(w)
v = QHBoxLayout()
# l = QFormLayout()
# v.addLayout(l)
l_coordsEdit = QLineEdit()
r_coordsEdit = QLineEdit()
lgps_label = QLabel('LGPS:')
rgps_label = QLabel('RGPS:')
v.addWidget(lgps_label)
v.addWidget(l_coordsEdit)
v.addWidget(rgps_label)
v.addWidget(r_coordsEdit)
h.addLayout(v)
# l.addRow('Coords:', coordsEdit)
# l.addRow('lcoords:',r_coordsEdit)
#h.addLayout(v)
#l = QFormLayout()
#h.addLayout(l)
#coordsEdit = QLineEdit()
#l.addRow('Coords:', coordsEdit)
#coordsEdit.editingFinished.connect(goCoords)
self.map = QOSM(self._widget)
self.map.mapMoved.connect(onMapMoved)
#self.map.markerMoved.connect(onMarkerMoved)
self.map.mapClicked.connect(onMapLClick)
self.map.mapDoubleClicked.connect(onMapDClick)
self.map.mapRightClicked.connect(onMapRClick)
self.map.markerClicked.connect(onMarkerLClick)
self.map.markerDoubleClicked.connect(onMarkerDClick)
self.map.markerRightClicked.connect(onMarkerRClick)
self.l_location.connect(move_l_mark)
self.r_location.connect(move_r_mark)
h.addWidget(self.map)
self.map.setSizePolicy(QSizePolicy.MinimumExpanding,
QSizePolicy.MinimumExpanding)
# 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('osm_maps_node'), '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')
# 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.map.waitUntilReady()
self.map.centerAt(32.7471012, -96.883642)
self.map.setZoom(12)
# Many icons at: https://sites.google.com/site/gmapsdevelopment/
coords = self.map.center()
self.map.addMarker(
"local GPS", *coords,
**dict(
icon=
"http://maps.gstatic.com/mapfiles/ridefinder-images/mm_20_gray.png",
draggable=True,
title="locat GPS marker!"))
coords = coords[0] + 0.1, coords[1] + 0.1
self.map.addMarker(
"remote GPS", *coords,
**dict(
icon=
"http://maps.gstatic.com/mapfiles/ridefinder-images/mm_20_red.png",
draggable=True,
title="remote GPS marker"))
sub1 = rospy.Subscriber("/local_gps", Float64MultiArray,
local_callback)
sub2 = rospy.Subscriber("/remote_gps", Float64MultiArray,
remote_callback)
#time.sleep(10)
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
class GraphViewWidget(QDockWidget):
'''
A frame to find text in the Editor.
'''
load_signal = Signal(str, bool)
''' @ivar: filename of file to load, True if insert after the current open tab'''
goto_signal = Signal(str, int)
''' @ivar: filename, line to go'''
DATA_FILE = Qt.UserRole + 1
DATA_LINE = Qt.UserRole + 2
DATA_INC_FILE = Qt.UserRole + 3
DATA_LEVEL = Qt.UserRole + 4
def __init__(self, tabwidget, parent=None):
QDockWidget.__init__(self, "LaunchGraph", parent)
graph_ui_file = os.path.join(
os.path.dirname(os.path.realpath(__file__)), 'GraphDockWidget.ui')
loadUi(graph_ui_file, self)
self.setObjectName('LaunchGraph')
self.setFeatures(QDockWidget.DockWidgetMovable
| QDockWidget.DockWidgetFloatable)
self._tabwidget = tabwidget
self._current_path = None
self._root_path = None
self._current_deep = 0
self.graphTreeView.setSelectionBehavior(QAbstractItemView.SelectRows)
model = QStandardItemModel()
self.graphTreeView.setModel(model)
self.graphTreeView.setUniformRowHeights(True)
self.graphTreeView.header().hide()
self.htmlDelegate = HTMLDelegate()
self.graphTreeView.setItemDelegateForColumn(0, self.htmlDelegate)
self.graphTreeView.activated.connect(self.on_activated)
self.graphTreeView.clicked.connect(self.on_clicked)
self._created_tree = False
self._refill_tree([], [], False)
def clear_cache(self):
with CHACHE_MUTEX:
GRAPH_CACHE.clear()
self._created_tree = False
self.graphTreeView.model().clear()
crp = self._current_path
self._current_path = None
self.set_file(crp, self._root_path)
def set_file(self, current_path, root_path):
self._root_path = root_path
if self._current_path != current_path:
self._current_path = current_path
# TODO: run analyzer/path parser in a new thread
self.setWindowTitle("Include Graph - loading...")
self._fill_graph_thread = GraphThread(current_path, root_path)
self._fill_graph_thread.graph.connect(self._refill_tree)
self._fill_graph_thread.start()
def find_parent_file(self):
selected = self.graphTreeView.selectionModel().selectedIndexes()
for index in selected:
item = self.graphTreeView.model().itemFromIndex(index.parent())
self.load_signal.emit(
item.data(self.DATA_INC_FILE),
self._current_deep < item.data(self.DATA_LEVEL))
def on_activated(self, index):
item = self.graphTreeView.model().itemFromIndex(index)
self.load_signal.emit(item.data(self.DATA_INC_FILE),
self._current_deep < item.data(self.DATA_LEVEL))
def on_clicked(self, index):
item = self.graphTreeView.model().itemFromIndex(index)
self.goto_signal.emit(item.data(self.DATA_FILE),
item.data(self.DATA_LINE))
def enable(self):
self.setVisible(True)
self.raise_()
self.activateWindow()
self.graphTreeView.setFocus()
def _refill_tree(self, included_from, includes, create_tree=True):
deep = 0
file_dsrc = self._root_path
try:
file_dsrc = os.path.basename(self._root_path)
except Exception:
pass
self.setWindowTitle("Include Graph - %s" % file_dsrc)
if not self._created_tree and create_tree:
with CHACHE_MUTEX:
if self._root_path in GRAPH_CACHE:
pkg, _ = package_name(os.path.dirname(self._root_path))
itemstr = '%s [%s]' % (os.path.basename(
self._root_path), pkg)
inc_item = QStandardItem('%s' % itemstr)
inc_item.setData(self._root_path, self.DATA_FILE)
inc_item.setData(-1, self.DATA_LINE)
inc_item.setData(self._root_path, self.DATA_INC_FILE)
inc_item.setData(deep, self.DATA_LEVEL)
self._append_items(inc_item, deep + 1)
self.graphTreeView.model().appendRow(inc_item)
# self.graphTreeView.expand(self.graphTreeView.model().indexFromItem(inc_item))
self._created_tree = True
items = self.graphTreeView.model().match(
self.graphTreeView.model().index(0, 0), self.DATA_INC_FILE,
self._current_path, 10, Qt.MatchRecursive)
first = True
self.graphTreeView.selectionModel().clearSelection()
for item in items:
if first:
self._current_deep = item.data(self.DATA_LEVEL)
self.graphTreeView.selectionModel().select(
item, QItemSelectionModel.SelectCurrent)
first = False
else:
self.graphTreeView.selectionModel().select(
item, QItemSelectionModel.Select)
self.graphTreeView.expandAll()
def _append_items(self, item, deep):
path = item.data(self.DATA_INC_FILE)
if not path:
path = item.data(self.DATA_FILE)
if path in GRAPH_CACHE:
for inc_lnr, inc_path in GRAPH_CACHE[path]:
pkg, _ = package_name(os.path.dirname(inc_path))
itemstr = '%s [%s]' % (os.path.basename(inc_path), pkg)
inc_item = QStandardItem('%d: %s' % (inc_lnr + 1, itemstr))
inc_item.setData(path, self.DATA_FILE)
inc_item.setData(inc_lnr + 1, self.DATA_LINE)
inc_item.setData(inc_path, self.DATA_INC_FILE)
inc_item.setData(deep, self.DATA_LEVEL)
self._append_items(inc_item, deep + 1)
item.appendRow(inc_item)
class TopicSelection(QWidget):
recordSettingsSelected = Signal(bool, list)
def __init__(self):
super(TopicSelection, self).__init__()
master = rosgraph.Master('rqt_bag_recorder')
self.setWindowTitle("Select the topics you want to record")
self.resize(500, 700)
self.topic_list = []
self.selected_topics = []
self.items_list = []
self.area = QScrollArea(self)
self.main_widget = QWidget(self.area)
self.ok_button = QPushButton("Record", self)
self.ok_button.clicked.connect(self.onButtonClicked)
self.ok_button.setEnabled(False)
self.from_nodes_button = QPushButton("From Nodes", self)
self.from_nodes_button.clicked.connect(self.onFromNodesButtonClicked)
self.main_vlayout = QVBoxLayout(self)
self.main_vlayout.addWidget(self.area)
self.main_vlayout.addWidget(self.ok_button)
self.main_vlayout.addWidget(self.from_nodes_button)
self.setLayout(self.main_vlayout)
self.selection_vlayout = QVBoxLayout(self)
self.item_all = QCheckBox("All", self)
self.item_monitor = QCheckBox("Monitor", self)
self.item_monAge = QCheckBox("Monitor + Agents", self)
self.item_all.stateChanged.connect(self.updateList)
self.item_monitor.stateChanged.connect(lambda x: self.updateList(x, "Monitor"))
self.item_monAge.stateChanged.connect(lambda x: self.updateList(x, "Monitor+"))
self.selection_vlayout.addWidget(self.item_all)
self.selection_vlayout.addWidget(self.item_monitor)
self.selection_vlayout.addWidget(self.item_monAge)
topic_data_list = master.getPublishedTopics('')
topic_data_list.sort()
for topic, datatype in topic_data_list:
self.addCheckBox(topic)
self.main_widget.setLayout(self.selection_vlayout)
self.area.setWidget(self.main_widget)
self.show()
def addCheckBox(self, topic):
self.topic_list.append(topic)
item = QCheckBox(topic, self)
item.stateChanged.connect(lambda x: self.updateList(x, topic))
self.items_list.append(item)
self.selection_vlayout.addWidget(item)
def changeTopicCheckState(self, topic, state):
for item in self.items_list:
if item.text() == topic:
item.setCheckState(state)
return
def updateList(self, state, topic=None, force_update_state=False):
if topic is None: # The "All" checkbox was checked / unchecked
if state == Qt.Checked:
self.item_all.setTristate(False)
for item in self.items_list:
if item.checkState() == Qt.Unchecked:
item.setCheckState(Qt.Checked)
elif state == Qt.Unchecked:
self.item_all.setTristate(False)
for item in self.items_list:
if item.checkState() == Qt.Checked:
item.setCheckState(Qt.Unchecked)
elif topic == "Monitor" or topic == "Monitor+":
if state == Qt.Checked:
self.item_monitor.setTristate(False)
self.selected_topics.append("/world_model")
self.selected_topics.append("/draws")
self.selected_topics.append("/debugs")
self.selected_topics.append("/referee")
for item in self.items_list:
if item.text() == "/world_model" \
or item.text() == "/debugs" \
or item.text() == "/draws" \
or item.text() =="/referee":
item.setCheckState(Qt.Checked)
elif state == Qt.Unchecked:
self.item_all.setTristate(False)
self.selected_topics.remove("/world_model")
self.selected_topics.remove("/draws")
self.selected_topics.remove("/debugs")
self.selected_topics.remove("/referee")
for item in self.items_list:
if item.text() == "/world_model" \
or item.text() =="/referee" \
or item.text() =="/debugs" \
or item.text()=="/draws":
item.setCheckState(Qt.Unchecked)
if topic == "Monitor+":
if state == Qt.Checked:
for item in self.items_list:
if item.text().find("agent") >-1:
self.selected_topics.append(item)
item.setCheckState(Qt.Checked)
elif state == Qt.Unchecked:
for item in self.items_list:
if item.text().find("agent") >-1:
self.selected_topics.remove(item)
item.setCheckState(Qt.Unchecked)
else:
if state == Qt.Checked:
self.selected_topics.append(topic)
else:
self.selected_topics.remove(topic)
if self.item_all.checkState() == Qt.Checked:
self.item_all.setCheckState(Qt.PartiallyChecked)
if self.selected_topics == []:
self.ok_button.setEnabled(False)
else:
self.ok_button.setEnabled(True)
def onButtonClicked(self):
self.close()
self.recordSettingsSelected.emit(
self.item_all.checkState() == Qt.Checked, self.selected_topics)
def onFromNodesButtonClicked(self):
self.node_selection = NodeSelection(self)
class ObstacleControlWorker(QObject):
finished = Signal() # class variable shared by all instances
def __init__(self,
control_callback,
finished_callback=None,
node_name='obstacle_display_worker',
frame_id='world',
marker_topic='obstacle_marker',
new_node=False,
parent=None):
super(ObstacleControlWorker, self).__init__(parent)
if new_node:
rospy.init_node(node_name, anonymous=True)
if finished_callback is not None:
self.finished.connect(finished_callback)
#TODO([email protected]) - how to shut down?
self.is_running = True
self.control_callback = control_callback
self.frame_id = frame_id
self.marker_server = InteractiveMarkerServer(marker_topic)
def make_obstacles(self, constraint_list):
self.marker_server.clear()
self.marker_server.applyChanges()
for i in range(0, len(constraint_list)):
if constraint_list[i].constraint_type != "ellipsoid":
# only plot ellipsoids
continue
if constraint_list[i].der != 0:
# Only plot physical constraints
continue
# create an interactive marker
int_marker = InteractiveMarker()
int_marker.header.frame_id = self.frame_id
int_marker.scale = 0.2
int_marker.pose.position.x = constraint_list[i].x0[0]
int_marker.pose.position.y = constraint_list[i].x0[1]
int_marker.pose.position.z = constraint_list[i].x0[2]
A = np.matrix(constraint_list[i].A)
if hasattr(constraint_list[i], "rot_mat"):
rot_mat = np.matrix(constraint_list[i].rot_mat)
scale = rot_mat * A * rot_mat.T
else:
rot_mat = np.matrix(np.identity(3))
scale = rot_mat * A * rot_mat.T
q = transforms3d.quaternions.mat2quat(rot_mat)
# NOTE: a differen convention with quaternions here
int_marker.pose.orientation.w = q[0]
int_marker.pose.orientation.x = q[1]
int_marker.pose.orientation.y = q[2]
int_marker.pose.orientation.z = q[3]
int_marker.name = str(i)
# create a non-interactive control which contains the box
obstacle_control = InteractiveMarkerControl()
obstacle_control.always_visible = True
obstacle_control.orientation_mode = InteractiveMarkerControl.VIEW_FACING
obstacle_control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
obstacle_control.independent_marker_orientation = True
obstacle_marker = self.build_obstacle_marker_template(
constraint_list[i])
obstacle_control.markers.append(obstacle_marker)
int_marker.controls.append(obstacle_control)
for ox, oy, oz, name in [(1, 0, 0, 'move_x'), (0, 1, 0, 'move_z'),
(0, 0, 1, 'move_y'),
(1, 0, 0, 'rotate_x'),
(0, 0, 1, 'rotate_y'),
(0, 1, 0, 'rotate_z')]:
#not sure why z and y are swapped
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = ox
control.orientation.y = oy
control.orientation.z = oz
control.name = name
if name[0] == 'm':
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
else:
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
# callback probably needs to be in this thread for ROS
# Then, that callback can use Qt signals and slots
self.marker_server.insert(int_marker,
self.obstacle_control_callback)
self.marker_server.applyChanges()
def update_obstacles(self, constraint_list, indices=None):
n_obstacles = len(constraint_list)
if indices is None:
indices = range(0, n_obstacles)
for i in indices:
if constraint_list[
i].constraint_type is "ellipsoid" and constraint_list[
i].der == 0:
pose = geometry_msgs.msg.Pose()
pose.position.x = constraint_list[i].x0[0]
pose.position.y = constraint_list[i].x0[1]
pose.position.z = constraint_list[i].x0[2]
A = np.matrix(constraint_list[i].A)
if hasattr(constraint_list[i], "rot_mat"):
rot_mat = np.matrix(constraint_list[i].rot_mat)
scale = rot_mat * A * rot_mat.T
else:
rot_mat = np.matrix(np.identity(3))
scale = rot_mat * A * rot_mat.T
q = transforms3d.quaternions.mat2quat(rot_mat)
# NOTE: a differen convention with quaternions here
pose.orientation.w = q[0]
pose.orientation.x = q[1]
pose.orientation.y = q[2]
pose.orientation.z = q[3]
self.marker_server.setPose(str(i), pose)
self.marker_server.applyChanges()
def index_from_name(self, name):
return int(re.search(r'\d+', name).group())
def obstacle_control_callback(self, callback):
#compute difference vector for this cube
position = dict()
position['x'] = callback.pose.position.x
position['y'] = callback.pose.position.y
position['z'] = callback.pose.position.z
orientation = dict()
orientation['w'] = callback.pose.orientation.w
orientation['x'] = callback.pose.orientation.x
orientation['y'] = callback.pose.orientation.y
orientation['z'] = callback.pose.orientation.z
index = self.index_from_name(callback.marker_name)
#if index > len(qr_p.waypoints['x'][0,:]):
# return
if callback.event_type == InteractiveMarkerFeedback.MOUSE_UP:
print("Obstacle control callback for index " + str(index))
if self.control_callback is not None:
self.control_callback(position, orientation, index)
#update_waypoint(position, None, index)
#qr_p.set_yaw_des_from_traj()
return
if callback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
# TODO([email protected]) - attach control_callback here
#update_waypoint(position, None, index, defer=True)
#qr_p.set_yaw_des_from_traj()
return
def build_obstacle_marker_template(self, constraint):
obstacle_marker = Marker()
obstacle_marker.type = obstacle_marker.SPHERE
A = np.matrix(constraint.A)
if hasattr(constraint, "rot_mat"):
rot_mat = np.matrix(constraint.rot_mat)
scale = rot_mat * A * rot_mat.T
else:
rot_mat = np.matrix(np.identity(3))
scale = rot_mat * A * rot_mat.T
# Size
obstacle_marker.scale.x = np.sqrt(1 / scale[0, 0])
obstacle_marker.scale.y = np.sqrt(1 / scale[1, 1])
obstacle_marker.scale.z = np.sqrt(1 / scale[2, 2])
obstacle_marker.color.r = 0.5
obstacle_marker.color.g = 0.5
obstacle_marker.color.b = 1
obstacle_marker.color.a = 0.7
return obstacle_marker
def stop(self):
self.is_running = False
class FilterChildrenModel(QSortFilterProxyModel):
"""
Extending QSortFilterProxyModel.
this provides methods to filter children
tree nodes.
QSortFilterProxyModel filters top-down direction starting from the
top-level of tree, and once a node doesn't hit the query it gets disabled.
Filtering with this class reflects the result from the bottom node.
Ex.
#TODO example needed here
"""
# Emitted when parameters filtered. int indicates the order/index of
# params displayed.
sig_filtered = Signal(int)
def __init__(self, parent):
super(FilterChildrenModel, self).__init__(parent)
# :Key: Internal ID of QModelIndex of each treenode.
# :Value: TreenodeStatus
# self._treenodes = OrderedDict()
self._parent = parent
self._toplv_parent_prev = None
def filterAcceptsRow(self, src_row, src_parent_qmindex):
"""
Overridden.
Terminology:
"Treenode" is deliberately used to avoid confusion with "Node" in ROS.
:type src_row: int
:type src_parent_qmindex: QModelIndex
"""
logging.debug('filerAcceptRow 1')
return self._filter_row_recur(src_row, src_parent_qmindex)
def _filter_row_recur(self, src_row, src_parent_qmindex):
"""
Filter row recursively.
:type src_row: int
:type src_parent_qmindex: QModelIndex
"""
_src_model = self.sourceModel()
curr_qmindex = _src_model.index(src_row, 0, src_parent_qmindex)
curr_qitem = _src_model.itemFromIndex(curr_qmindex)
if isinstance(curr_qitem, TreenodeQstdItem):
# If selectable ROS Node, get GRN name
nodename_fullpath = curr_qitem.get_raw_param_name()
text_filter_target = nodename_fullpath
logging.debug(' Nodename full={} '.format(nodename_fullpath))
else:
# If ReadonlyItem, this means items are the parameters, not a part
# of node name. So, get param name.
text_filter_target = curr_qitem.data(Qt.DisplayRole)
regex = self.filterRegExp()
pos_hit = regex.indexIn(text_filter_target)
if pos_hit >= 0: # Query hit.
logging.debug('curr data={} row={} col={}'.format(
curr_qmindex.data(), curr_qmindex.row(), curr_qmindex.column()
))
# Set all subsequent treenodes True
logging.debug(
' FCModel.filterAcceptsRow'
' src_row={} parent row={} data={} filterRegExp={}'.format(
src_row, src_parent_qmindex.row(),
src_parent_qmindex.data(), regex))
# If the index is the terminal treenode, parameters that hit
# the query are displayed at the root tree.
_child_index = curr_qmindex.child(0, 0)
if ((not _child_index.isValid()) and
(isinstance(curr_qitem, TreenodeQstdItem))):
self._show_params_view(src_row, curr_qitem)
# Once we find a treenode that hits the query, no need to further
# traverse since what this method wants to know with the given
# index is whether the given index is supposed to be shown or not.
# Thus, just return True here.
return True
if not isinstance(curr_qitem, TreenodeQstdItem):
return False # If parameters, no need for recursive filtering.
# Evaluate children recursively.
row_child = 0
while True:
child_qmindex = curr_qmindex.child(row_child, 0)
if child_qmindex.isValid():
flag = self._filter_row_recur(row_child, curr_qmindex)
if flag:
return True
else:
return False
row_child += 1
return False
def _show_params_view(self, src_row, curr_qitem):
logging.debug('_show_params_view data={}'.format(
curr_qitem.data(Qt.DisplayRole)
))
curr_qitem.enable_param_items()
def _get_toplevel_parent_recur(self, qmindex):
p = qmindex.parent()
if p.isValid():
self._get_toplevel_parent(p)
return p
def filterAcceptsColumn(self, source_column, source_parent):
"""
Overridden.
Doing nothing really since columns are not in use.
:type source_column: int
:type source_parent: QModelIndex
"""
logging.debug('FCModel.filterAcceptsCol source_col={} '.format(
source_column) + 'parent col={} row={} data={}'.format(
source_parent.column(), source_parent.row(),
source_parent.data()))
return True
def set_filter(self, filter_):
self._filter = filter_
# If filtered text is '' (0-length str), invalidate current
# filtering, in the hope of making filtering process faster.
if filter_.get_text == '':
self.invalidate()
logging.info('filter invalidated.')
# By calling setFilterRegExp, filterAccepts* methods get kicked.
self.setFilterRegExp(self._filter.get_regexp())
class WaypointControlWorker(QObject):
finished = Signal() # class variable shared by all instances
def __init__(self,
control_callback,
menu_callback=None,
finished_callback=None,
node_name='waypoint_control_worker',
frame_id='world',
marker_topic='trajectory_control',
new_node=False,
parent=None,
use_menu=True,
qr_type="main",
entry_ID=0,
exit_ID=0):
super(WaypointControlWorker, self).__init__(parent)
if new_node:
rospy.init_node(node_name, anonymous=True)
if finished_callback is not None:
self.finished.connect(finished_callback)
#TODO([email protected]) - how to shut down?
self.is_running = True
self.use_menu = use_menu
self.qr_type = qr_type
self.entry_ID = entry_ID
self.exit_ID = exit_ID
self.control_callback = control_callback
self.menu_callback = menu_callback
self.frame_id = frame_id
self.marker_server = InteractiveMarkerServer(marker_topic)
# if self.use_menu:
# self.init_menu()
# TODO([email protected]) - how to maximize reusability?
# what waypoint format - full pose? or x,y,z,yaw?
def make_controls(self, waypoints, closed_loop=False):
self.marker_server.clear()
self.marker_server.applyChanges()
# Set tracking of interactive waypoint states
self.check_tracking = np.zeros(len(waypoints['x'][0, :]))
self.check_tracking[self.entry_ID] += 1
self.check_tracking[self.exit_ID] += 2
# Create menus for each waypoint
self.init_menu(waypoints)
print("Creating " + str(len(waypoints['x'][0, :])) +
" trajectory controls")
for i in range(0, len(waypoints['x'][0, :])):
if closed_loop and i == (len(waypoints['x'][0, :]) - 1):
print("ignoring last waypoint")
continue
# Do not create last waypoint control
if self.qr_type == 'entry' and i == (len(waypoints['x'][0, :]) -
1):
# Don't move the waypoint that is fixed to the trajectory
continue
if self.qr_type == 'exit' and i == 0:
# Don't move the waypoint that is fixed to the trajectory
continue
# create an interactive marker
int_marker = InteractiveMarker()
int_marker.header.frame_id = self.frame_id
int_marker.scale = 0.2
int_marker.pose.position.x = waypoints['x'][0, i]
int_marker.pose.position.y = waypoints['y'][0, i]
int_marker.pose.position.z = waypoints['z'][0, i]
# q = tf.transformations.quaternion_from_euler(1.5,0,waypoints['yaw'][0,i])
# int_marker.pose.orientation.x = q[0]
# int_marker.pose.orientation.y = q[1]
# int_marker.pose.orientation.z = q[2]
# int_marker.pose.orientation.w = q[3]
int_marker.name = str(i) + self.qr_type
# create a non-interactive control which contains the box
quad_control = InteractiveMarkerControl()
quad_control.always_visible = True
quad_control.orientation_mode = InteractiveMarkerControl.VIEW_FACING
quad_control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
quad_control.independent_marker_orientation = True
quad_marker = build_quad_marker_template()
if self.qr_type is "main" and i != self.entry_ID and i != self.exit_ID:
quad_marker.color.r = 1.0
quad_marker.color.g = 0.5
quad_marker.color.b = 0.5
elif self.qr_type is "entry" or (i == self.entry_ID
and self.qr_type is "main"):
quad_marker.color.r = 0.5
quad_marker.color.g = 1.0
quad_marker.color.b = 0.5
elif self.qr_type is "exit" or (i == self.exit_ID
and self.qr_type is "main"):
quad_marker.color.r = 1.0
quad_marker.color.g = 0.0
quad_marker.color.b = 0.0
quad_control.markers.append(quad_marker)
int_marker.controls.append(quad_control)
for ox, oy, oz, name in [(1, 0, 0, 'move_x'), (0, 1, 0, 'move_z'),
(0, 0, 1, 'move_y'),
(0, 1, 0, 'rotate_z')]:
#not sure why z and y are swapped
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = ox
control.orientation.y = oy
control.orientation.z = oz
control.name = name
if name[0] == 'm':
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
else:
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
if self.use_menu:
# Initialise the controls
menu_control = InteractiveMarkerControl()
# Set the interaction mode
menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
menu_control.always_visible = True
menu_control.name = 'menu'
# Append a marker to the control
menu_control.markers.append(quad_marker)
# Append the controls to the interactive marker
int_marker.controls.append(menu_control)
# callback probably needs to be in this thread for ROS
# Then, that callback can use Qt signals and slots
self.marker_server.insert(int_marker,
self.waypoint_control_callback)
if self.use_menu:
self.menu_handler[str(i)].apply(self.marker_server,
int_marker.name)
# if True:#i==0:
# # Call again to ativate both menu and move-interactivity for the first waypoint
# self.marker_server.insert( int_marker, self.waypoint_control_callback )
# self.menu_handler[key].apply( self.marker_server, int_marker.name)
self.marker_server.applyChanges()
# TODO([email protected]) - how to maximize reusability?
# what waypoint format - full pose? or x,y,z,yaw?
# who is reponsible for recording a change in type of marker?
def update_controls(self,
waypoints,
indices=None,
closed_loop=False,
acc_wp=None):
n_waypoints = len(waypoints['x'][0, :])
if indices is None:
indices = range(0, n_waypoints)
if closed_loop:
# Do not create a marker for the last waypoint
indices = np.delete(indices, np.where(indices == n_waypoints - 1))
for i in indices:
if closed_loop and i == (len(waypoints['x'][0, :]) - 1):
print("ignoring last waypoint")
continue
# Do not create last waypoint control
if self.qr_type == 'entry' and i == (len(waypoints['x'][0, :]) -
1):
# Don't move the waypoint that is fixed to the trajectory
continue
if self.qr_type == 'exit' and i == 0:
# Don't move the waypoint that is fixed to the trajectory
continue
pose = geometry_msgs.msg.Pose()
pose.position.x = waypoints['x'][0, i]
pose.position.y = waypoints['y'][0, i]
pose.position.z = waypoints['z'][0, i]
if acc_wp is None:
q = tf.transformations.quaternion_from_euler(
0.0, 0.0, waypoints['yaw'][0, i])
pose.orientation.w = q[3]
pose.orientation.x = q[0]
pose.orientation.y = q[1]
pose.orientation.z = q[2]
else:
q, data = body_frame.body_frame_from_yaw_and_accel(
waypoints['yaw'][0, i],
acc_wp[:, i],
out_format='quaternion',
deriv_type='yaw_only')
pose.orientation.w = q[0]
pose.orientation.x = q[1]
pose.orientation.y = q[2]
pose.orientation.z = q[3]
self.marker_server.setPose(str(i) + self.qr_type, pose)
self.marker_server.applyChanges()
def index_from_name(self, name):
return int(re.search(r'\d+', name).group())
def waypoint_control_callback(self, callback):
#compute difference vector for this cube
position = dict()
position['x'] = callback.pose.position.x
position['y'] = callback.pose.position.y
position['z'] = callback.pose.position.z
index = self.index_from_name(callback.marker_name)
#if index > len(qr_p.waypoints['x'][0,:]):
# return
if callback.event_type == InteractiveMarkerFeedback.MOUSE_UP:
print("Waypoint control callback for index " + str(index))
if self.control_callback is not None:
self.control_callback(position, index, self.qr_type)
#update_waypoint(position, None, index)
#qr_p.set_yaw_des_from_traj()
return
if callback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
# TODO([email protected]) - attach control_callback here
#update_waypoint(position, None, index, defer=True)
#qr_p.set_yaw_des_from_traj()
return
def init_menu(self, waypoints):
self.menu_handler = dict()
self.num_waypoints = len(waypoints['x'][0, :])
for i in range(0, self.num_waypoints):
key = str(i)
self.menu_handler[key] = MenuHandler()
# Insert waypoint button
self.insert_entry_id = 1 # menu ID for insert
h_insert = self.menu_handler[key].insert("Insert Waypoint")
self.menu_handler[key].insert("Before",
parent=h_insert,
callback=self.insert_cb)
self.menu_handler[key].insert("After",
parent=h_insert,
callback=self.insert_cb)
# Delete waypoint button
self.menu_handler[key].insert("Delete Waypoint",
callback=self.delete_cb)
# Menu to select waypoint type
top_level = self.menu_handler[key].insert("Change Type")
if self.qr_type is "main":
# Checkbox menu items
self.normal_option_ID = 6
h_type = self.menu_handler[key].insert(
"Normal", parent=top_level, callback=self.change_type_cb)
if self.check_tracking[i] == 0:
self.menu_handler[key].setCheckState(
h_type, MenuHandler.CHECKED)
else:
self.menu_handler[key].setCheckState(
h_type, MenuHandler.UNCHECKED)
self.entry_option_ID = 7
h_type = self.menu_handler[key].insert(
"Entry", parent=top_level, callback=self.change_type_cb)
if self.check_tracking[i] == 1 or self.check_tracking[i] == 3:
self.menu_handler[key].setCheckState(
h_type, MenuHandler.CHECKED)
else:
self.menu_handler[key].setCheckState(
h_type, MenuHandler.UNCHECKED)
self.exit_option_ID = 8
h_type = self.menu_handler[key].insert(
"Exit", parent=top_level, callback=self.change_type_cb)
if self.check_tracking[i] == 2 or self.check_tracking[i] == 3:
self.menu_handler[key].setCheckState(
h_type, MenuHandler.CHECKED)
else:
self.menu_handler[key].setCheckState(
h_type, MenuHandler.UNCHECKED)
elif self.qr_type is "entry":
# Checkbox menu items
h_type = self.menu_handler[key].insert(
"Take-off", parent=top_level, callback=self.change_type_cb)
self.menu_handler[key].setCheckState(h_type,
MenuHandler.CHECKED)
elif self.qr_type is "exit":
# Checkbox menu items
h_type = self.menu_handler[key].insert(
"Landing", parent=top_level, callback=self.change_type_cb)
self.menu_handler[key].setCheckState(h_type,
MenuHandler.CHECKED)
self.marker_server.applyChanges()
def delete_cb(self, callback):
# get the index
index = self.index_from_name(callback.marker_name)
# rospy.loginfo("Deleting waypoint {}".format(index))
if index <= self.entry_ID:
# Reset entry if point deleted before or at the current entry waypoint
self.add_entry_property(self.entry_ID - 1)
if index <= self.exit_ID:
# Reset exit if point deleted before or at the current exit waypoint
self.add_exit_property(self.exit_ID - 1)
# Command type for callback
command_type = "delete"
# Group information in data dict for callback
data = dict()
data['index'] = index
data['entry_ID'] = self.entry_ID
data['exit_ID'] = self.exit_ID
# Callback
self.menu_callback(command_type, data, self.qr_type)
def insert_cb(self, callback):
# get the index of the marker
if callback.menu_entry_id == self.insert_entry_id + 1:
# Insert before the current waypoint
index = self.index_from_name(callback.marker_name)
else:
# Insert after the current waypoint
index = self.index_from_name(callback.marker_name) + 1
if index <= self.entry_ID:
# Reset entry if point deleted before or at the current entry waypoint
self.add_entry_property(self.entry_ID + 1)
if index <= self.exit_ID:
# Reset exit if point deleted before or at the current exit waypoint
self.add_exit_property(self.exit_ID + 1)
command_type = "insert"
data = dict()
data['index'] = index
data['entry_ID'] = self.entry_ID
data['exit_ID'] = self.exit_ID
# Callback
self.menu_callback(command_type, data, self.qr_type)
def change_type_cb(self, callback):
# get the index
index = self.index_from_name(callback.marker_name)
key = str(index)
# Check on
if self.qr_type is not "main":
return
# handle for the menu entry
handle = callback.menu_entry_id
# DIfferent handle options to consider
options = np.array(
[self.normal_option_ID, self.entry_option_ID, self.exit_option_ID])
# Get checked state
state = self.menu_handler[key].getCheckState(handle)
if handle == self.entry_option_ID:
# Entry
if state == MenuHandler.CHECKED:
self.remove_entry_property(index)
else:
self.add_entry_property(index)
command_type = "change_entry"
elif handle == self.exit_option_ID:
# exit
if state == MenuHandler.CHECKED:
self.remove_exit_property(index)
else:
self.add_exit_property(index)
command_type = "change_exit"
else:
# Normal input
if state == MenuHandler.CHECKED:
# do nothing
return
if self.check_tracking[index] == 3:
# both
self.remove_entry_property(index)
self.remove_exit_property(index)
command_type = "change_both"
elif self.check_tracking[index] == 1:
self.remove_entry_property(index)
command_type = "change_entry"
else:
self.remove_exit_property(index)
command_type = "change_exit"
# Apply the changes
self.menu_handler[key].reApply(self.marker_server)
self.marker_server.applyChanges()
# Send data through callback
data = dict()
data['index'] = index
data['entry_ID'] = self.entry_ID
data['exit_ID'] = self.exit_ID
self.menu_callback(command_type, data, self.qr_type)
# if self.menu_callback is not None:
# self.menu_callback(data)
def add_entry_property(self, index):
for i in range(0, self.num_waypoints):
if i == index:
# add entry property
self.check_tracking[i] += 1
elif self.check_tracking[i] == 1 or self.check_tracking[i] == 3:
# Not Neutral - take away property
self.check_tracking[i] -= 1
self.entry_ID = index
def add_exit_property(self, index):
for i in range(0, self.num_waypoints):
if i == index:
# add entry property
self.check_tracking[i] += 2
elif self.check_tracking[i] >= 2:
# Not Neutral - take away property
self.check_tracking[i] -= 2
self.exit_ID = index
def remove_entry_property(self, index):
self.check_tracking[index] -= 1
# Reset entry ID to 0 TODO([email protected]) review if this is the best way to do it
if index != 0:
self.entry_ID = 0
self.check_tracking[0] += 1
else:
self.entry_ID = 1
self.check_tracking[1] += 1
def remove_exit_property(self, index):
self.check_tracking[index] -= 2
# Reset entry ID to 0 TODO([email protected]) review if this is the best way to do it
if index != 0:
self.exit_ID = 0
self.check_tracking[0] += 2
else:
self.exit_ID = 1
self.check_tracking[1] += 2
def stop(self):
self.is_running = False
class JoyModel(QObject):
buttonSignal0 = Signal(bool)
buttonSignal1 = Signal(bool)
buttonSignal2 = Signal(bool)
buttonSignal3 = Signal(bool)
buttonSignal4 = Signal(bool)
buttonSignal5 = Signal(bool)
buttonSignal6 = Signal(bool)
buttonSignal7 = Signal(bool)
buttonSignal8 = Signal(bool)
buttonSignal9 = Signal(bool)
buttonSignal10 = Signal(bool)
axisSignal0 = Signal(float)
axisSignal1 = Signal(float)
axisSignal2 = Signal(float)
axisSignal3 = Signal(float)
axisSignal4 = Signal(float)
axisSignal5 = Signal(float)
axisSignal6 = Signal(float)
axisSignal7 = Signal(float)
def __init__(self, parent=None):
super(JoyModel, self).__init__(parent)
self._is_connected = False
self._joySubscriber = rospy.Subscriber('tauv_joy', Joy,
self.joyCallback)
self._connectionService = rospy.ServiceProxy('joy_node/connect',
JoyConnect)
self._disconnectionService = rospy.ServiceProxy(
'joy_node/disconnect', Trigger)
self._shutdownService = rospy.ServiceProxy('joy_node/shutdown',
Trigger)
self._devname = ""
self._devpath = "/dev/input/"
def __del__(self):
print("deleting joystick visualizer.")
self.doDisconnect()
self._joySubscriber.unregister()
self._connectionService.close()
self._disconnectionService.close()
self._shutdownService.close()
@Slot()
def doConnect(self):
if self._is_connected:
self.doDisconnect()
try:
rospy.wait_for_service("joy_node/connect", 0.2)
except rospy.ROSException, e:
rospy.logerr(
"Cannot connect Joystick! No Joystick Node Running: %s", e)
return
#
# jc = JoyConnect()
# jc.dev = "asdf"#str(self._devpath + self._devname)
self._connectionService(self._devpath + self._devname)
class AnimateWorker(QObject):
finished = Signal() # class variable shared by all instances
def __init__(self,
eval_callback,
dt=0.01,
finished_callback=None,
node_name='animate_worker',
frame_id='animation',
parent_frame_id='world',
marker_topic='animation_marker',
new_node=False,
slowdown=1,
parent=None):
super(AnimateWorker, self).__init__(parent)
if new_node:
rospy.init_node(node_name, anonymous=True)
if finished_callback is not None:
self.finished.connect(finished_callback)
#TODO([email protected]) - how to shut down?
self.is_running = True
self.publish = True
self.animate = False
self.eval_callback = eval_callback
self.t = 0.
self.t_max = 0.
self.dt = dt
if slowdown <= 0.001: # prevent speeding up by more than 1000x
slowdown = 1
self.slowdown = slowdown
self.R_old = None
self.frame_id = frame_id
self.parent_frame_id = parent_frame_id
self.tf_br = tf.TransformBroadcaster()
self.marker_pub = rospy.Publisher(marker_topic,
MarkerArray,
queue_size=1)
self.refresh_marker()
print("init animate")
self.timer = QTimer()
self.timer.setInterval(self.dt * 1000 *
self.slowdown) # in milliseconds
#self.timer.setTimerType(Qt.PreciseTimer)
self.timer.timeout.connect(self.on_timer_callback)
self.timer.start()
def refresh_marker(self):
quad_marker = build_quad_marker_template()
quad_marker.id = 0
quad_marker.header.frame_id = self.frame_id
quad_marker.frame_locked = True
quad_marker.action = quad_marker.ADD
quad_marker.pose.orientation.w = 1
quad_marker.pose.position.x = 0.0
quad_marker.pose.position.y = 0.0
quad_marker.pose.position.z = 0.0
quad_marker.lifetime = rospy.Duration(0)
marker_array = MarkerArray()
marker_array.markers.append(quad_marker)
self.marker_pub.publish(marker_array)
def on_timer_callback(self):
if not self.publish:
return
try:
if self.animate:
self.t += self.dt
if self.t > self.t_max:
self.t = 0.
else:
self.t = 0.
(t_max, pos_vec, qw, qx, qy, qz, yaw, vel_vec,
acc_vec) = self.eval_callback(self.t)
self.t_max = t_max
self.tf_br.sendTransform(
(pos_vec[0], pos_vec[1], pos_vec[2]), (qx, qy, qz, qw),
rospy.Time.now(), self.frame_id, self.parent_frame_id)
#except Exception as e:
#print("Error in AnimateWorker")
#print(e)
except:
# print("Unknown error in AnimateWorker")
return
def start_animate(self):
print("Ros helper start animation")
self.animate = True
self.refresh_marker()
def stop_animate(self):
self.animate = False
def start_publish(self):
self.t = 0
self.publish = True
self.refresh_marker()
def stop_publish(self):
self.publish = False
class QInteractionsChooser(QMainWindow):
# pyqt signals are always defined as class attributes
signal_interactions_updated = Signal()
def __init__(self, parent, title, application, rocon_master_index="", rocon_master_name="", rocon_master_uri='localhost', host_name='localhost'):
super(QInteractionsChooser, self).__init__(parent)
self.rocon_master_index = rocon_master_index
self.rocon_master_uri = rocon_master_uri
self.rocon_master_name = rocon_master_name
self.host_name = host_name
self.cur_selected_interaction = None
self.cur_selected_role = 0
self.interactions = {}
self.interactive_client = InteractiveClient(stop_interaction_postexec_fn=self.interactions_updated_relay)
self.application = application
rospack = rospkg.RosPack()
icon_file = os.path.join(rospack.get_path('rocon_icons'), 'icons', 'rocon_logo.png')
self.application.setWindowIcon(QIcon(icon_file))
self.interactions_widget = QWidget()
self.roles_widget = QWidget()
path = os.path.join(os.path.dirname(os.path.abspath(__file__)), "../../ui/interactions_list.ui")
loadUi(path, self.interactions_widget)
path = os.path.join(os.path.dirname(os.path.abspath(__file__)), "../../ui/role_list.ui")
loadUi(path, self.roles_widget)
# role list widget
self.roles_widget.role_list_widget.setIconSize(QSize(50, 50))
self.roles_widget.role_list_widget.itemDoubleClicked.connect(self._switch_to_interactions_list)
self.roles_widget.back_btn.pressed.connect(self._switch_to_master_chooser)
self.roles_widget.stop_all_interactions_button.pressed.connect(self._stop_all_interactions)
self.roles_widget.refresh_btn.pressed.connect(self._refresh_role_list)
self.roles_widget.closeEvent = self._close_event
# interactions list widget
self.interactions_widget.interactions_list_widget.setIconSize(QSize(50, 50))
self.interactions_widget.interactions_list_widget.itemDoubleClicked.connect(self._start_interaction)
self.interactions_widget.back_btn.pressed.connect(self._switch_to_role_list)
self.interactions_widget.interactions_list_widget.itemClicked.connect(self._display_interaction_info) # rocon master item click event
self.interactions_widget.stop_interactions_button.pressed.connect(self._stop_interaction)
self.interactions_widget.stop_interactions_button.setDisabled(True)
self.interactions_widget.closeEvent = self._close_event
# signals
self.signal_interactions_updated.connect(self._refresh_interactions_list, Qt.QueuedConnection)
self.signal_interactions_updated.connect(self._set_stop_interactions_button, Qt.QueuedConnection)
# create a few directories for caching icons and ...
utils.setup_home_dirs()
# connect to the ros master
(result, message) = self.interactive_client._connect(self.rocon_master_name, self.rocon_master_uri, self.host_name)
if not result:
QMessageBox.warning(self, 'Connection Failed', "%s." % message.capitalize(), QMessageBox.Ok)
self._switch_to_master_chooser()
return
role_list = self._refresh_role_list()
# Ugly Hack : our window manager is not graying out the button when an interaction closes itself down and the appropriate
# callback (_set_stop_interactions_button) is fired. It does otherwise though so it looks like the window manager
# is getting confused when the original program doesn't have the focus.
#
# Taking control of it ourselves works...
self.interactions_widget.stop_interactions_button.setStyleSheet("QPushButton:disabled { color: gray }")
# show interactions list if there's no choice amongst roles, otherwise show the role list
if len(role_list) == 1:
self.cur_selected_role = role_list[0]
self.interactive_client.select_role(self.cur_selected_role)
self.interactions_widget.show()
self._refresh_interactions_list()
else:
self.roles_widget.show()
def shutdown(self):
"""
Public method to enable shutdown of the script - this function is primarily for
shutting down the interactions chooser from external signals (e.g. CTRL-C on the command
line).
"""
self.interactive_client.shutdown()
def _close_event(self, event):
"""
Re-implementation of close event handlers for the interaction chooser's children
(i.e. role and interactions list widgets).
"""
console.logdebug("Interactions Chooser : remocon shutting down.")
self.shutdown()
######################################
# Roles List Widget
######################################
def _switch_to_interactions_list(self, Item):
"""
Take the selected role and switch to an interactions view of that role.
"""
console.logdebug("Interactions Chooser : switching to the interactions list")
self.cur_selected_role = str(Item.text())
self.interactive_client.select_role(self.cur_selected_role)
self.interactions_widget.show()
self.interactions_widget.move(self.roles_widget.pos())
self.roles_widget.hide()
self._refresh_interactions_list()
def _switch_to_master_chooser(self):
console.logdebug("Interactions Chooser : switching back to the master chooser")
self.shutdown()
os.execv(QMasterChooser.rocon_remocon_script, ['', self.host_name])
def _refresh_role_list(self):
self.roles_widget.role_list_widget.clear()
role_list = self.interactive_client.get_role_list()
#set list widget item (reverse order because we push them on the top)
for role in reversed(role_list):
self.roles_widget.role_list_widget.insertItem(0, role)
#setting the list font
font = self.roles_widget.role_list_widget.item(0).font()
font.setPointSize(13)
self.roles_widget.role_list_widget.item(0).setFont(font)
return role_list
def _stop_all_interactions(self):
console.logdebug("Interactions Chooser : stopping all running interactions")
self.interactive_client.stop_all_interactions()
self.roles_widget.stop_all_interactions_button.setEnabled(False)
######################################
# Interactions List Widget
######################################
def _switch_to_role_list(self):
console.logdebug("Interactions Chooser : switching to the role list")
# show the roles widget
if self.interactive_client.has_running_interactions():
self.roles_widget.stop_all_interactions_button.setEnabled(True)
else:
self.roles_widget.stop_all_interactions_button.setEnabled(False)
self.roles_widget.show()
self.roles_widget.move(self.interactions_widget.pos())
# show the interactions widget
self.interactions_widget.stop_interactions_button.setEnabled(False)
self.interactions_widget.hide()
def _display_interaction_info(self, Item):
"""
Display the currently selected interaction's information. Triggered
when single-clicking on it in the interactions list view.
"""
list_widget = Item.listWidget()
cur_index = list_widget.count() - list_widget.currentRow() - 1
for k in self.interactions.values():
if(k.index == cur_index):
self.cur_selected_interaction = k
break
self.interactions_widget.app_info.clear()
info_text = "<html>"
info_text += "<p>-------------------------------------------</p>"
web_interaction = web_interactions.parse(self.cur_selected_interaction.name)
name = self.cur_selected_interaction.name if web_interaction is None else web_interaction.url
info_text += "<p><b>name: </b>" + name + "</p>"
info_text += "<p><b> ---------------------</b>" + "</p>"
info_text += "<p><b>compatibility: </b>" + self.cur_selected_interaction.compatibility + "</p>"
info_text += "<p><b>display name: </b>" + self.cur_selected_interaction.display_name + "</p>"
info_text += "<p><b>description: </b>" + self.cur_selected_interaction.description + "</p>"
info_text += "<p><b>namespace: </b>" + self.cur_selected_interaction.namespace + "</p>"
info_text += "<p><b>max: </b>" + str(self.cur_selected_interaction.max) + "</p>"
info_text += "<p><b> ---------------------</b>" + "</p>"
info_text += "<p><b>remappings: </b>" + str(self.cur_selected_interaction.remappings) + "</p>"
info_text += "<p><b>parameters: </b>" + str(self.cur_selected_interaction.parameters) + "</p>"
info_text += "</html>"
self.interactions_widget.app_info.appendHtml(info_text)
self._set_stop_interactions_button()
######################################
# Gui Updates/Refreshes
######################################
def interactions_updated_relay(self):
"""
Called by the underlying interactive client whenever the gui needs to be updated with
fresh information. Using this relay saves us from having to embed qt functions in the
underlying class but makes sure we signal across threads so the gui can update things
in its own thread.
Currently this only handles updates caused by termination of an interaction. If we wished to
handle additional situations, we should use an argument here indicating what kind of interaction
update occurred.
"""
self.signal_interactions_updated.emit()
# this connects to:
# - self._refresh_interactions_list()
# - self._set_stop_interactions_button()
def _refresh_interactions_list(self):
"""
This just does a complete redraw of the interactions with the
currently selected role. It's a bit brute force doing this
every time the interactions' 'state' changes, but this suffices for now.
:param str role_name: role to request list of interactions for.
"""
console.logdebug("Interactions Chooser : refreshing the interactions list")
self.interactions = self.interactive_client.interactions(self.cur_selected_role)
self.interactions_widget.interactions_list_widget.clear()
index = 0
for interaction in self.interactions.values():
interaction.index = index
index = index + 1
self.interactions_widget.interactions_list_widget.insertItem(0, interaction.display_name)
# is it a currently running pairing
if self.interactive_client.pairing == interaction.hash:
self.interactions_widget.interactions_list_widget.item(0).setBackground(QColor(100, 100, 150))
#setting the list font
font = self.interactions_widget.interactions_list_widget.item(0).font()
font.setPointSize(13)
self.interactions_widget.interactions_list_widget.item(0).setFont(font)
#setting the icon
icon = interaction.icon
if icon == "unknown.png":
icon = QIcon(self.icon_paths['unknown'])
self.interactions_widget.interactions_list_widget.item(0).setIcon(icon)
elif len(icon):
icon = QIcon(os.path.join(utils.get_icon_cache_home(), icon))
self.interactions_widget.interactions_list_widget.item(0).setIcon(icon)
else:
console.logdebug("%s : No icon" % str(self.rocon_master_name))
def _set_stop_interactions_button(self):
'''
Disable or enable the stop button depending on whether the
selected interaction has any currently launched processes,
'''
if not self.interactions:
console.logwarn("No interactions")
return
if self.cur_selected_interaction.launch_list:
console.logdebug("Interactions Chooser : enabling stop interactions button [%s]" % self.cur_selected_interaction.display_name)
self.interactions_widget.stop_interactions_button.setEnabled(True)
else:
console.logdebug("Interactions Chooser : disabling stop interactions button [%s]" % self.cur_selected_interaction.display_name)
self.interactions_widget.stop_interactions_button.setEnabled(False)
######################################
# Start/Stop Interactions
######################################
def _start_interaction(self):
console.logdebug("Interactions Chooser : starting interaction [%s]" % str(self.cur_selected_interaction.name))
(result, message) = self.interactive_client.start_interaction(self.cur_selected_role,
self.cur_selected_interaction.hash)
if result:
if self.cur_selected_interaction.is_paired_type():
self._refresh_interactions_list() # make sure the highlight is working
self.interactions_widget.stop_interactions_button.setDisabled(False)
else:
QMessageBox.warning(self, 'Start Interaction Failed', "%s." % message.capitalize(), QMessageBox.Ok)
console.logwarn("Interactions Chooser : start interaction failed [%s]" % message)
def _stop_interaction(self):
console.logdebug("Interactions Chooser : stopping interaction %s " % str(self.cur_selected_interaction.name))
(result, message) = self.interactive_client.stop_interaction(self.cur_selected_interaction.hash)
if result:
if self.cur_selected_interaction.is_paired_type():
self._refresh_interactions_list() # make sure the highlight is disabled
self._set_stop_interactions_button()
#self.interactions_widget.stop_interactions_button.setDisabled(True)
else:
QMessageBox.warning(self, 'Stop Interaction Failed', "%s." % message.capitalize(), QMessageBox.Ok)
console.logwarn("Interactions Chooser : stop interaction failed [%s]" % message)
