class MissionServer(object): def __init__(self, plan_names, master_timeout=None): self._plans = PlanSet(plan_names) self._master_timeout = master_timeout self._sm = None self._shared = uf_smach.util.StateSharedHandles() self._pub = rospy.Publisher('mission/plans', PlansStamped) self._srv = rospy.Service('mission/modify_plan', ModifyPlan, self._modify_plan) self._run_srv = actionlib.SimpleActionServer('mission/run', RunMissionsAction, self.execute, False) self._run_srv.register_preempt_callback(self._on_preempt) self._run_srv.start() self._tim = rospy.Timer(rospy.Duration(.1), lambda _: self._publish_plans()) self._kill_listener = KillListener(self._on_preempt) def get_plan(self, plan): return self._plans.get_plan(plan) def execute(self, goal): self._sm = self._plans.make_sm(self._shared, self._master_timeout) sis = smach_ros.IntrospectionServer('mission_planner', self._sm, '/SM_ROOT') sis.start() print 'Waiting for unkill' while self._kill_listener.get_killed(): rospy.sleep(.1) outcome = self._sm.execute() sis.stop() self._shared['moveto'].cancel_goal() if outcome == 'succeeded': self._run_srv.set_succeeded(RunMissionsResult(outcome)) else: self._run_srv.set_preempted() def _on_preempt(self): if self._sm is not None: self._sm.request_preempt() def _publish_plans(self): self._pub.publish(PlansStamped( header=Header(stamp=rospy.Time.now()), plans=[Plan(name=name, entries=[uf_smach.msg.PlanEntry(entry.mission, rospy.Duration(entry.timeout), entry.contigency_plan, entry.path, entry.dist) for entry in self._plans.get_plan(name)]) for name in self._plans.get_plans()], available_missions=get_missions())) def _modify_plan(self, req): if req.plan not in self._plans.get_plans(): return None plan = self._plans.get_plan(req.plan) entry = PlanEntry(req.entry.mission, req.entry.timeout.to_sec(), req.entry.contigency_plan if len(req.entry.contigency_plan) > 0 else None, req.entry.path if req.entry.path != 'none' else None, req.entry.dist) if req.operation == ModifyPlanRequest.INSERT: if req.pos > len(plan): return None plan.insert(req.pos, entry) elif req.operation == ModifyPlanRequest.REMOVE: if req.pos >= len(plan): return None del plan[req.pos] elif req.operation == ModifyPlanRequest.REPLACE: if req.pos >= len(plan): return None plan[req.pos] = entry else: return None return ()
class PropaGatorGUI(Plugin): def __init__(self, contex): super(PropaGatorGUI, self).__init__(contex) # Initilize variables self._float_status = False self.last_odom_msg = Odometry() self._current_controller = 'Controller: Unknown' self._controller = '' self._controllers = [] # Assign a name self.setObjectName('PropaGatorGUI') # Create a widget self._widget = QWidget() loadUi(os.path.join(cwd, 'propagatorgui.ui'), self._widget) self._widget.setObjectName('PropaGatorGUI') contex.add_widget(self._widget) # Grab all the children from the widget self._kill_label = self._widget.findChild(QLabel, 'kill_label') self._float_label = self._widget.findChild(QLabel, 'float_label') self._controller_label = self._widget.findChild( QLabel, 'controller_label') self._odom_x_label = self._widget.findChild(QLabel, 'odom_x_label') self._odom_y_label = self._widget.findChild(QLabel, 'odom_y_label') self._odom_yaw_label = self._widget.findChild(QLabel, 'odom_yaw_label') self._odom_d_x_label = self._widget.findChild(QLabel, 'odom_d_x_label') self._odom_d_y_label = self._widget.findChild(QLabel, 'odom_d_y_label') self._odom_d_yaw_label = self._widget.findChild( QLabel, 'odom_d_yaw_label') self._placeholder_label = self._widget.findChild( QLabel, 'placeholder_label') self._kill_push_btn = self._widget.findChild(QPushButton, 'kill_push_btn') self._float_push_btn = self._widget.findChild(QPushButton, 'float_push_btn') self._controller_combo_box = self._widget.findChild( QComboBox, 'controller_combo_box') # Load images self._green_indicator = QPixmap( os.path.join(cwd, 'green_indicator.png')) self._red_indicator = QPixmap(os.path.join(cwd, 'red_indicator.png')) self._placeholder_image = QPixmap(os.path.join(cwd, 'placeholder.png')) # Set up ROS interfaces self._kill_listener = KillListener() self._kill_broadcaster = KillBroadcaster( id='PropaGator GUI', description='PropaGator GUI kill') self._odom_sub = rospy.Subscriber('/odom', Odometry, self._odom_cb) self._float_sub = rospy.Subscriber('/controller/float_status', Bool, self._float_cb) self._float_proxy = rospy.ServiceProxy('/controller/float_mode', FloatMode) self._current_controller_sub = rospy.Subscriber( '/controller/current_controller', String, self._current_controller_cb) self._get_all_controllers_proxy = rospy.ServiceProxy( '/controller/get_all_controllers', get_all_controllers) self._request_controller_proxy = rospy.ServiceProxy( '/controller/request_controller', request_controller) # Connect push buttons self._kill_push_btn.toggled.connect(self._on_kill_push_btn_toggle) self._float_push_btn.toggled.connect(self._on_float_push_btn_toggle) # Connect combo boxes self._controller_combo_box.activated[str].connect( self._controller_combo_box_cb) # Set up update timer at 10Hz # A Qt timer is used instead of a ros timer since Qt components are updated self.update_timer = QTimer() self.update_timer.timeout.connect(self._onUpdate) self.update_timer.start(100) # Temp self._placeholder_label.setPixmap(self._placeholder_image) # Everything needs to be turned off here def shutdown_plugin(self): self.update_timer.stop() self._odom_sub.unregister() del self._odom_sub self._float_sub.unregister() del self._float_sub # Kill broadcaster is not cleared, the user should unkill before closing the GUI del self._kill_broadcaster del self._kill_listener # Subscriber callbacks # Since this is in a different thread it is possible and likely that # the drawing thread will try and draw while the text is being changed # this causes all kinds of mahem such as segmentation faults, double free, ... # To prevent this from hapening this thread updates only none QT variables # described here http://wiki.ros.org/rqt/Tutorials/Writing%20a%20Python%20Plugin def _odom_cb(self, msg): self.last_odom_msg = msg def _float_cb(self, status): self._float_status = status.data def _current_controller_cb(self, controller): self._controller = controller.data self._current_controller = 'Controller: ' + controller.data def _odom_update(self): pos = self.last_odom_msg.pose.pose.position yaw = quat_to_rotvec( xyzw_array(self.last_odom_msg.pose.pose.orientation))[2] vel = self.last_odom_msg.twist.twist.linear dYaw = self.last_odom_msg.twist.twist.angular.z self._odom_x_label.setText('X: %3.3f' % pos.x) self._odom_y_label.setText('Y: %3.3f' % pos.y) self._odom_yaw_label.setText('Yaw: %3.3f' % yaw) self._odom_d_x_label.setText('dX: %3.3f' % vel.x) self._odom_d_y_label.setText('dY: %3.3f' % vel.y) self._odom_d_yaw_label.setText('dYaw: %3.3f' % dYaw) # Push btn callbacks def _on_kill_push_btn_toggle(self, checked): if checked: self._kill_broadcaster.send(True) else: self._kill_broadcaster.send(False) def _on_float_push_btn_toggle(self, checked): # TODO: Check if float service active try: if checked: self._float_proxy(True) else: self._float_proxy(False) except rospy.service.ServiceException: pass # Combo box callbacks def _controller_combo_box_cb(self, text): self._request_controller_proxy(text) # Update functions def _updateStatus(self): # Check if killed if self._kill_listener.get_killed(): self._kill_label.setPixmap(self._red_indicator) else: self._kill_label.setPixmap(self._green_indicator) # Check float status if self._float_status: self._float_label.setPixmap(self._red_indicator) else: self._float_label.setPixmap(self._green_indicator) # Check if in autonomous or RC self._controller_label.setText(self._current_controller) def _updateControl(self): # Wait until we get the first controller if self._controller == '': return controllers = self._get_all_controllers_proxy().controllers if controllers != self._controllers: self._controllers = controllers self._controller_combo_box.clear() self._controller_combo_box.addItems(controllers) index = self._controller_combo_box.findText(self._controller) if index != -1 and index != self._controller_combo_box.currentIndex(): self._controller_combo_box.setCurrentIndex(index) def _updateLidar(self): pass def _onUpdate(self): self._updateStatus() self._updateControl() self._updateLidar() self._odom_update()
class KillPlugin(Plugin): def __init__(self, context): super(KillPlugin, self).__init__(context) self.setObjectName('KillPlugin') self._listener = KillListener() self._broadcaster = KillBroadcaster(rospy.get_name(), 'Software kill using KillPlugin') self._kill_active = False self._widget = QWidget() loadUi(os.path.join(uipath, 'killplugin.ui'), self._widget) context.add_widget(self._widget) self._widget.findChild(QTableWidget, 'killTable').setHorizontalHeaderLabels( ["Name", "Status", "Description"]) self._widget.findChild(QPushButton, 'killButton').clicked.connect( self._on_kill_clicked) self._widget.findChild(QPushButton, 'unkillButton').clicked.connect( self._on_unkill_clicked) self._widget.findChild(QPushButton, 'runButton').clicked.connect( self._on_run_clicked) self._update_timer = QTimer(self._widget) self._update_timer.timeout.connect(self._on_update) self._update_timer.start(100) def _on_update(self): self._update_kills() self._update_kill() def _on_kill_clicked(self): self._kill_active = True self._broadcaster.send(True) self._update_kill() def _on_unkill_clicked(self): self._kill_active = False self._broadcaster.send(False) self._update_kill() def _on_run_clicked(self): self._on_unkill_clicked() def _update_kills(self): all_kills = self._listener.get_all_kills() if all_kills is not None: new_kills = { kill.id: kill for kill in self._listener.get_all_kills() } else: new_kills = {} table = self._widget.findChild(QTableWidget, 'killTable') row = 0 while row < table.rowCount(): id = table.item(row, 1) if id in new_kills: self._update_kill_entry(row, new_kills[id]) del new_kills[id] row += 1 else: table.removeRow(row) for kill in new_kills.values(): row = table.rowCount() table.setRowCount(row + 1) self._update_kill_entry(row, kill) def _update_kill_entry(self, row, kill): color = QColor(255, 255, 255) if not kill.active else QColor( 255, 200, 200) self._update_item(row, 0, color, kill.id) self._update_item(row, 1, color, "Killed" if kill.active else "Unkilled") self._update_item(row, 2, color, kill.description) def _update_item(self, row, col, color, string): item = QTableWidgetItem(string) item.setBackground(color) self._widget.findChild(QTableWidget, 'killTable').setItem(row, col, item) def _update_kill(self): kills = self._listener.get_all_kills() if kills is not None: other_kill_count = len([ kill for kill in kills if kill.id != rospy.get_name() and kill.active ]) self._widget.findChild( QPushButton, 'runButton').setVisible(other_kill_count == 0) self._widget.findChild( QPushButton, 'unkillButton').setVisible(other_kill_count > 0) status = 'Sub status: ' if not self._listener.get_killed(): status += '<span style="color:green">Running</span>' else: status += '<span style="color:red">Killed</span>' self._widget.findChild(QLabel, 'killStatusLabel').setText(status) else: self._widget.findChild(QPushButton, 'runButton').setVisible(False) self._widget.findChild(QPushButton, 'unkillButton').setVisible(False) self._widget.findChild(QLabel, 'killStatusLabel').setText( '<span style="color:red">No kill information</span>')
class KillPlugin(Plugin): def __init__(self, context): super(KillPlugin, self).__init__(context) self.setObjectName('KillPlugin') self._listener = KillListener() self._broadcaster = KillBroadcaster(rospy.get_name(), 'Software kill using KillPlugin') self._kill_active = False self._widget = QWidget() loadUi(os.path.join(uipath, 'killplugin.ui'), self._widget) context.add_widget(self._widget) self._widget.findChild(QTableWidget, 'killTable').setHorizontalHeaderLabels(["Name", "Status", "Description"]) self._widget.findChild(QPushButton, 'killButton').clicked.connect(self._on_kill_clicked) self._widget.findChild(QPushButton, 'unkillButton').clicked.connect(self._on_unkill_clicked) self._widget.findChild(QPushButton, 'runButton').clicked.connect(self._on_run_clicked) self._update_timer = QTimer(self._widget) self._update_timer.timeout.connect(self._on_update) self._update_timer.start(100) def _on_update(self): self._update_kills() self._update_kill() def _on_kill_clicked(self): self._kill_active = True self._broadcaster.send(True) self._update_kill() def _on_unkill_clicked(self): self._kill_active = False self._broadcaster.send(False) self._update_kill() def _on_run_clicked(self): self._on_unkill_clicked() def _update_kills(self): all_kills = self._listener.get_all_kills() if all_kills is not None: new_kills = {kill.id: kill for kill in self._listener.get_all_kills()} else: new_kills = {} table = self._widget.findChild(QTableWidget, 'killTable') row = 0 while row < table.rowCount(): id = table.item(row, 1) if id in new_kills: self._update_kill_entry(row, new_kills[id]) del new_kills[id] row += 1 else: table.removeRow(row) for kill in new_kills.values(): row = table.rowCount() table.setRowCount(row+1) self._update_kill_entry(row, kill) def _update_kill_entry(self, row, kill): color = QColor(255, 255, 255) if not kill.active else QColor(255, 200, 200) self._update_item(row, 0, color, kill.id) self._update_item(row, 1, color, "Killed" if kill.active else "Unkilled") self._update_item(row, 2, color, kill.description) def _update_item(self, row, col, color, string): item = QTableWidgetItem(string) item.setBackground(color) self._widget.findChild(QTableWidget, 'killTable').setItem(row, col, item) def _update_kill(self): kills = self._listener.get_all_kills() if kills is not None: other_kill_count = len([kill for kill in kills if kill.id != rospy.get_name() and kill.active]) self._widget.findChild(QPushButton, 'runButton').setVisible(other_kill_count == 0) self._widget.findChild(QPushButton, 'unkillButton').setVisible(other_kill_count > 0) status = 'Sub status: ' if not self._listener.get_killed(): status += '<span style="color:green">Running</span>' else: status += '<span style="color:red">Killed</span>' self._widget.findChild(QLabel, 'killStatusLabel').setText(status) else: self._widget.findChild(QPushButton, 'runButton').setVisible(False) self._widget.findChild(QPushButton, 'unkillButton').setVisible(False) self._widget.findChild(QLabel, 'killStatusLabel').setText('<span style="color:red">No kill information</span>')