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)