class InspectorWidget(QWidget): write = pyqtSignal(str, str) newline = pyqtSignal() clear = pyqtSignal() def __init__(self, status): super(InspectorWidget, self).__init__() self.status = status self.setWindowTitle(status.name) self.paused = False layout = QVBoxLayout() self.disp = QTextEdit() self.snapshot = QPushButton("Snapshot") self.time = TimelineWidget(self) layout.addWidget(self.disp, 1) layout.addWidget(self.time, 0) layout.addWidget(self.snapshot) self.snaps = [] self.snapshot.clicked.connect(self.take_snapshot) self.write.connect(self.write_kv) self.newline.connect(lambda: self.disp.insertPlainText('\n')) self.clear.connect(lambda: self.disp.clear()) self.setLayout(layout) self.setGeometry(0,0,300,400) self.show() self.update(status) def write_kv(self, k, v): self.disp.setFontWeight(75) self.disp.insertPlainText(k) self.disp.insertPlainText(': ') self.disp.setFontWeight(50) self.disp.insertPlainText(v) self.disp.insertPlainText('\n') def pause(self, msg): self.update(msg); self.paused = True def unpause(self): self.paused = False def update(self, status): if not self.paused: self.status = status self.time.add_message(status) self.clear.emit() self.write.emit("Full Name", status.name) self.write.emit("Component", status.name.split('/')[-1]) self.write.emit("Hardware ID", status.hardware_id) self.write.emit("Level", str(status.level)) self.write.emit("Message", status.message) self.newline.emit() for v in status.values: self.write.emit(v.key, v.value) def take_snapshot(self): snap = Snapshot(self.status) self.snaps.append(snap)
class Widgetti(QWidget): dead_update = Signal() pose_update = Signal() def __init__(self): super(Widgetti, self).__init__() self.layout = QVBoxLayout() self.control_layout = QVBoxLayout() self.button_layout = QHBoxLayout() self.map_layout = QHBoxLayout() self.console_layout = QVBoxLayout() self.text_layout = QHBoxLayout() self.tf = tf.TransformListener() self.dead_update.connect(self.update_dead) self.pose_update.connect(self.update_pose_in_map) self.pirates = [] self.dead_pirates = [] self.dead_pirate_objects = [] self.pirate_update = True self.dead_pirate_update = False self.pose = None self.waiting = False #self.pirate_detector = pirate_detector() #Initializing pirate detector self.setWindowTitle('GUI for Pioneer P3-DX') self.gui_publisher = rospy.Publisher('gui_plan', Path) self.actionclient = actionlib.SimpleActionClient('move_base', MoveBaseAction) #self.notificationPub = rospy.Publisher('notification', RideNotification) self.actionclient.wait_for_server() self.debug_stream = QTextEdit(self) self.taskplanner = TaskPlanner(parent = self) self.robomap = RoboMap(tf = self.tf, parent=self) self.left = QPushButton('Spin left') self.left.clicked.connect(self.spin_left) self.right = QPushButton('Spin right') self.right.clicked.connect(self.spin_right) self.point_move = QPushButton('Move to point') self.point_move.clicked.connect(self.point_go) self.control_layout.addWidget(self.left) self.control_layout.addWidget(self.right) self.control_layout.addWidget(self.point_move) # task planner stuff self.open_manip = QPushButton('Open manipulator') self.open_manip.clicked.connect(self.taskplanner.openManipulator) self.close_manip = QPushButton('Close manipulator') self.close_manip.clicked.connect(self.taskplanner.closeManipulator) self.taskplanning = QPushButton('Execute Mission') self.taskplanning.clicked.connect(self.taskplanner.execute) self.button_layout.addWidget(self.open_manip) self.button_layout.addWidget(self.close_manip) self.button_layout.addWidget(self.taskplanning) self.button_layout.addWidget(self.open_manip) self.button_layout.addWidget(self.close_manip) self.map_layout.addWidget(self.robomap) self.console_layout.addLayout(self.map_layout) self.console_layout.addLayout(self.control_layout) self.text_layout.addWidget(self.debug_stream) self.layout.addLayout(self.console_layout) self.layout.addLayout(self.button_layout) self.layout.addLayout(self.text_layout) self.layout.addWidget(QLabel('Graphical interface to visualize stuff')) self.setLayout(self.layout) self.timer = 0 self.pose_sub = rospy.Subscriber('RosAria/pose', Odometry, self.pose_callback) # Pirate detector subscribers self.pirates_sub = rospy.Subscriber('/Pirates', Path, self.pirate_callback) self.dead_pirates_sub = rospy.Subscriber('/Dead', Path, self.dead_pirate_callback) self.pirate_update = True self.dead_pirate_update = True self.pose_update_timer = 0 self.point_goal = None def spin_left(self): r = rospy.Rate(1.0) # 1 Hz movement = Twist() movement.angular.z = 3.14 # ~45 deg/s for i in range(32): self.taskplanner.driver.publish(movement) r.sleep() self.taskplanner.driver.publish(Twist()) def spin_right(self): r = rospy.Rate(1.0) # 1 Hz movement = Twist() movement.angular.z = -3.14 # ~45 deg/s for i in range(320): self.taskplanner.driver.publish(movement) r.sleep() self.taskplanner.driver.publish(Twist()) def point_go(self): if self.robomap.point: w = self.robomap.w res = self.robomap.resolution org = self.robomap.origin tmp = self.robomap.point.rect().center() x1 = (w - tmp.x()) * res + org[0] y1 = tmp.y() * res + org[1] x2 = (w - tmp.x()) * res + org[0] y2 = tmp.y() * res + org[1] quaternion = quaternion_from_euler(0, 0, math.atan2(y2 - y1, x2 - x1)) pose = PoseStamped() pose.header.stamp = rospy.Time.now() pose.header.frame_id = "map" pose.pose.position.x = x1 pose.pose.position.y = y1 pose.pose.orientation.w = quaternion[3] pose.pose.orientation.z = quaternion[2] self.point_goal = MoveBaseGoal(target_pose=pose) self.actionclient.send_goal(self.point_goal, feedback_cb=self.point_feedback) else: self.update_textbox('CANNOT EXECUTE MOVE: ', 'NO POINT SELECTED') def point_feedback(self, feedback): pose_stamp = feedback.base_position self.timer += 1 if self.pose_update_timer > 20: self.update_pose_in_map() self.pose_update_timer = 0 if self.point_goal and self.distance(self.point_goal, pose_stamp) < 0.2: self.timer = 0 #print str(self.distance(self.pirate_detector.move_goal, pose_stamp)) # this check cancels the goal if the robot is "close enough" # move_base will endlessly spin sometimes without this code self.actionclient.cancel_goal() rospy.sleep(1.0) self.point_goal = None if self.timer > 500: self.actionclient.cancel_goal() #self.update_textbox('Could not reach target: ', 'Timeout') self.pose_update_timer += 1 def pirate_callback(self, data): if self.pirate_update: for z in data.poses: self.pirates.append(z) self.pirate_update = False print self.pirates # Explorer callback #self.taskplanner.explorer.detector_callback(data) def dead_pirate_callback(self, data): if self.dead_pirate_update: for z in data.poses: self.dead_pirates.append(z) self.dead_pirate_update = False self.dead_update.emit() def update_dead(self): print 'Adding dead to map' if self.dead_pirate_objects: self.clear_dead() self.robomap.update_map(self.dead_pirates) #self.update_textbox('DEAD FOUND IN TOTAL: ', str(len(self.dead_pirates))) def clear_dead(self): for z in self.dead_pirate_objects: self.robomap.scene.removeItem(z) return True def update_textbox(self, header, txt): self.debug_stream.insertPlainText(header + '\n') self.debug_stream.insertPlainText(txt+'\n') def pose_callback(self, data): self.pose = data def update_pose_in_map(self): if self.robomap.point: self.robomap.scene.removeItem(self.robomap.point) self.robomap.point = None x = self.pose.pose.pose.position.x y = self.pose.pose.pose.position.y #transform pose coordinates to map coordinates map_y = (y - self.robomap.origin[1])/self.robomap.resolution map_x = -((x - self.robomap.origin[0])/self.robomap.resolution) + self.robomap.w self.robomap.point = self.robomap.draw_point(map_x, map_y, color=Qt.blue, rad=3.0) def done_callback(self, status, result): if status is GoalStatus.RECALLED: print 'recalled' elif status is GoalStatus.SUCCEEDED: print 'success' elif status is GoalStatus.REJECTED: print 'rejected' elif status is GoalStatus.ABORTED: print 'aborted' else: print 'sumthing else' print goal_states.get(status) def feedback(self, feedback): pose_stamp = feedback.base_position self.timer += 1 print 'in feedback' if self.pose_update_timer > 20: print 'omg' self.pose_update.emit() self.pose_update_timer = 0 if self.taskplanner.move_goal and self.distance(self.taskplanner.move_goal, pose_stamp) < 0.2: self.timer = 0 #print str(self.distance(self.pirate_detector.move_goal, pose_stamp)) # this check cancels the goal if the robot is "close enough" # move_base will endlessly spin sometimes without this code self.actionclient.cancel_goal() if self.taskplanner.state == -1: self.pirate_update = True self.dead_pirate_update = True rospy.sleep(1.0) self.goal = None print 'Moving to next state from ' + str(self.taskplanner.state) self.taskplanner.state = self.taskplanner.state + 1 self.waiting = False #self.taskplanner.explorer_pub = rospy.Publisher('explore_next_point', String, latch=False) if self.timer > 500: print 'wtf?' self.actionclient.cancel_goal() self.goal = None self.taskplanner.state = self.taskplanner.state + 1 self.waiting = False self.timer = 0 self.pose_update_timer += 1 def feedback2(self, feedback): pose_stamp = feedback.base_position self.timer += 1 print 'in feedback2' if self.taskplanner.move_goal and self.distance(self.taskplanner.move_goal, pose_stamp) < 0.2: self.timer = 0 #print str(self.distance(self.pirate_detector.move_goal, pose_stamp)) # this check cancels the goal if the robot is "close enough" # move_base will endlessly spin sometimes without this code self.actionclient.cancel_goal() rospy.sleep(1.0) self.taskplanner.move_goal = None luku = randint(0,2) r = rospy.Rate(1.0) # 1 Hz movement = Twist() if luku == 0: movement.angular.z = 0 if luku == 1: movement.angular.z = 3.14/2 # ~45 deg/s if luku == 2: movement.angular.z = -3.14/2 # ~45 deg/s self.taskplanner.driver.publish(movement) r.sleep() self.taskplanner.driver.publish(Twist()) rospy.sleep(2.0) self.pirate_update = True self.dead_pirate_update = True rospy.sleep(2.0) self.last_pirate = None if self.timer > 500: print 'wtf?' self.actionclient.cancel_goal() def distance(self, t1, t2): """ Given two PoseStamped's, determine the distance """ out = 0 for dimension in ('x', 'y'): out += math.pow(getattr(t1.target_pose.pose.position, dimension) - getattr(t2.pose.position, dimension), 2) return math.sqrt(out)
class InspectorWindow(AbstractStatusWidget): _sig_write = Signal(str, str) _sig_newline = Signal() _sig_close_window = Signal() _sig_clear = Signal() def __init__(self, status, close_callback): """ :type status: DiagnosticStatus :param close_callback: When the instance of this class (InspectorWindow) terminates, this callback gets called. """ #TODO(Isaac) UI construction that currently is done in this method, # needs to be done in .ui file. super(InspectorWindow, self).__init__() self.status = status self._close_callback = close_callback self.setWindowTitle(status.name) self.paused = False self.layout_vertical = QVBoxLayout(self) self.disp = QTextEdit(self) self.snapshot = QPushButton("StatusSnapshot") self.timeline_pane = TimelinePane(self) self.timeline_pane.set_timeline_data(Util.SECONDS_TIMELINE, self.get_color_for_value, self.on_pause) self.layout_vertical.addWidget(self.disp, 1) self.layout_vertical.addWidget(self.timeline_pane, 0) self.layout_vertical.addWidget(self.snapshot) self.snaps = [] self.snapshot.clicked.connect(self._take_snapshot) self._sig_write.connect(self._write_key_val) self._sig_newline.connect(lambda: self.disp.insertPlainText('\n')) self._sig_clear.connect(lambda: self.disp.clear()) self._sig_close_window.connect(self._close_callback) self.setLayout(self.layout_vertical) # TODO better to be configurable where to appear. self.setGeometry(0, 0, 400, 600) self.show() self.update_status_display(status) def closeEvent(self, event): # emit signal that should be slotted by StatusItem self._sig_close_window.emit() self.close() def _write_key_val(self, k, v): self.disp.setFontWeight(75) self.disp.insertPlainText(k) self.disp.insertPlainText(': ') self.disp.setFontWeight(50) self.disp.insertPlainText(v) self.disp.insertPlainText('\n') def pause(self, msg): rospy.logdebug('InspectorWin pause PAUSED') self.paused = True self.update_status_display(msg) def unpause(self, msg): rospy.logdebug('InspectorWin pause UN-PAUSED') self.paused = False def new_diagnostic(self, msg, is_forced=False): """ Overridden from AbstractStatusWidget :type status: DiagnosticsStatus """ if not self.paused: self.update_status_display(msg) rospy.logdebug('InspectorWin _cb len of queue=%d self.paused=%s', len(self.timeline_pane._queue_diagnostic), self.paused) else: if is_forced: self.update_status_display(msg, True) rospy.logdebug('@@@InspectorWin _cb PAUSED window updated') else: rospy.logdebug('@@@InspectorWin _cb PAUSED not updated') def update_status_display(self, status, is_forced=False): """ :type status: DiagnosticsStatus """ if not self.paused or (self.paused and is_forced): scroll_value = self.disp.verticalScrollBar().value() self.timeline_pane.new_diagnostic(status) rospy.logdebug('InspectorWin update_status_display 1') self.status = status self._sig_clear.emit() self._sig_write.emit("Full Name", status.name) self._sig_write.emit("Component", status.name.split('/')[-1]) self._sig_write.emit("Hardware ID", status.hardware_id) self._sig_write.emit("Level", str(status.level)) self._sig_write.emit("Message", status.message) self._sig_newline.emit() for v in status.values: self._sig_write.emit(v.key, v.value) if self.disp.verticalScrollBar().maximum() < scroll_value: scroll_value = self.disp.verticalScrollBar().maximum() self.disp.verticalScrollBar().setValue(scroll_value) def _take_snapshot(self): snap = StatusSnapshot(self.status) self.snaps.append(snap) def get_color_for_value(self, queue_diagnostic, color_index): """ Overridden from AbstractStatusWidget. :type color_index: int """ rospy.logdebug( 'InspectorWindow get_color_for_value ' + 'queue_diagnostic=%d, color_index=%d', len(queue_diagnostic), color_index) lv_index = queue_diagnostic[color_index - 1].level return Util.COLOR_DICT[lv_index]
class InspectorWindow(AbstractStatusWidget): _sig_write = Signal(str, str) _sig_newline = Signal() _sig_close_window = Signal() _sig_clear = Signal() def __init__(self, status, close_callback): """ @todo: UI construction that currently is done in this method, needs to be done in .ui file. @param status: DiagnosticStatus @param close_callback: When the instance of this class (InspectorWindow) terminates, this callback gets called. """ super(InspectorWindow, self).__init__() self.status = status self._close_callback = close_callback self.setWindowTitle(status.name) self.paused = False self.layout_vertical = QVBoxLayout(self) self.disp = QTextEdit(self) self.snapshot = QPushButton("Snapshot") self.timeline_pane = TimelinePane(self, Util._SECONDS_TIMELINE, self._cb, self.get_color_for_value) self.layout_vertical.addWidget(self.disp, 1) self.layout_vertical.addWidget(self.timeline_pane, 0) self.layout_vertical.addWidget(self.snapshot) self.snaps = [] self.snapshot.clicked.connect(self._take_snapshot) self._sig_write.connect(self._write_key_val) self._sig_newline.connect(lambda: self.disp.insertPlainText('\n')) self._sig_clear.connect(lambda: self.disp.clear()) self._sig_close_window.connect(self._close_callback) self.setLayout(self.layout_vertical) self.setGeometry( 0, 0, 400, 600) # TODO better to be configurable where to appear. self.show() self.update_status_display(status) def get_color_for_value(self, queue_diagnostic, color_index): rospy.logdebug( 'InspectorWindow get_color_for_value ' + 'queue_diagnostic=%d, color_index=%d', len(queue_diagnostic), color_index) lv_index = queue_diagnostic[color_index - 1].level return Util._COLOR_DICT[lv_index] ''' Delegated from super class. @author: Isaac Saito ''' def closeEvent(self, event): # emit signal that should be slotted by StatusItem self._sig_close_window.emit() self.close() def _write_key_val(self, k, v): self.disp.setFontWeight(75) self.disp.insertPlainText(k) self.disp.insertPlainText(': ') self.disp.setFontWeight(50) self.disp.insertPlainText(v) self.disp.insertPlainText('\n') def pause(self, msg): """ @todo: Create a superclass for this and RobotMonitorWidget that has pause func. """ rospy.logdebug('InspectorWin pause PAUSED') self.paused = True self.update_status_display(msg) def unpause(self, msg): rospy.logdebug('InspectorWin pause UN-PAUSED') self.paused = False #self.update_status_display(msg); def _cb(self, msg, is_forced=False): """ @param status: DiagnosticsStatus Overriden """ if not self.paused: #if is_forced: self.update_status_display(msg) rospy.logdebug('InspectorWin _cb len of queue=%d self.paused=%s', len(self.timeline_pane._queue_diagnostic), self.paused) else: if is_forced: self.update_status_display(msg, True) rospy.logdebug('@@@InspectorWin _cb PAUSED window updated') else: rospy.logdebug('@@@InspectorWin _cb PAUSED not updated') def update_status_display(self, status, is_forced=False): """ @param status: DiagnosticsStatus """ if not self.paused or (self.paused and is_forced): self.timeline_pane.new_diagnostic(status) rospy.logdebug('InspectorWin update_status_display 1') self.status = status self._sig_clear.emit() self._sig_write.emit("Full Name", status.name) self._sig_write.emit("Component", status.name.split('/')[-1]) self._sig_write.emit("Hardware ID", status.hardware_id) self._sig_write.emit("Level", str(status.level)) self._sig_write.emit("Message", status.message) self._sig_newline.emit() for v in status.values: self._sig_write.emit(v.key, v.value) def _take_snapshot(self): snap = Snapshot(self.status) self.snaps.append(snap) def enable(self): #wx.Panel.Enable(self) #self._timeline.enable() self.setEnabled(True) self.timeline_pane.enable() self.timeline_pane.pause_button.setDown(False) def disable(self): """Supposed to be called upon pausing.""" #wx.Panel.Disable(self) #self._timeline.Disable() self.setEnable(False) self.timeline_pane.disable() self.pause_button.Disable() self.unpause() self.timeline_pane.pause_button.setDown(True)