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)
