예제 #1
0
class NetworkCheck(object):
    def __init__(self, timeout=5.0, autonomous_msgs_req=50):
        self.timeout = rospy.Duration(timeout)
        self.last_time = rospy.Time.now()
        self.last_msg = ''
        
        # Make sure we don't accidentally let the sub loose.
        # We need auto_msgs_req messages before we go autonomous mode.
        self.auto_msgs_req = autonomous_msgs_req
        self.auto_msg_count = 0

        self.sub = rospy.Subscriber('/keep_alive', String, self.got_network_msg, queue_size=1)
        self.auto_service = rospy.ServiceProxy('/go_auto', Empty)
        self.kb = KillBroadcaster(id='network', description='Network timeout')
        #self.alarm_broadcaster, self.alarm = single_alarm('network-timeout', severity=1)
        rospy.Timer(rospy.Duration(0.1), self.check)

    def check(self, *args):
        if self.need_kill() and self.last_msg != '':
            if self.auto_msg_count >= self.auto_msgs_req:
                rospy.loginfo("AUTONOMOUS MODE STARTED")
                self.auto_service()

                # Kill the sub after the mission
                self.last_msg = 'keep_alive'
                self.auto_msg_count = 0

            #self.alarm.raise_alarm()
            rospy.logerr("KILLING SUB")
            self.kb.send(active=True)
        else:
            self.kb.send(active=False)
            #self.alarm.clear_alarm()

    def got_network_msg(self, msg):
        self.last_msg = msg.data
        self.last_time = rospy.Time.now()

        if msg.data == 'auto':
            if self.auto_msg_count == self.auto_msgs_req:
                rospy.loginfo("AUTONOMOUS MODE ARMED")

            self.auto_msg_count += 1
        else:
            self.auto_msg_count = 0

    def need_kill(self):
        return ((rospy.Time.now() - self.last_time) > self.timeout)
예제 #2
0
파일: kill.py 프로젝트: jpanikulam/Sub8
class Handler(object):
    alarm_name = 'kill'

    def __init__(self):
        # Keep some knowledge of which thrusters we have working
        self.kb = KillBroadcaster(id='alarm-kill', description='Kill by alarm')
        self.alarms = {}

    def handle(self, time_sent, parameters, alarm_name):
        self.alarms[alarm_name] = True
        self.kb.send(active=True)

    def cancel(self, time_sent, parameters, alarm_name):
        self.alarms[alarm_name] = False

        # Make sure that ALL alarms that caused a kill have been cleared
        if not any(self.alarms.values()):
            self.kb.send(active=False)
예제 #3
0
class Joystick(object):
    def __init__(self):

        self.force_scale = rospy.get_param("~force_scale", 600)
        self.torque_scale = rospy.get_param("~torque_scale", 500)

        self.wrench_choices = itertools.cycle(['rc', 'autonomous'])
        self.current_pose = Odometry()

        self.active = False

        self.alarm_broadcaster = AlarmBroadcaster()
        self.kill_alarm = self.alarm_broadcaster.add_alarm(
            name='kill', action_required=True, severity=0)

        # self.docking_alarm = self.alarm_broadcaster.add_alarm(
        #     name='docking',
        #     action_required=True,
        #     severity=0
        # )

        self.wrench_pub = rospy.Publisher("/wrench/rc",
                                          WrenchStamped,
                                          queue_size=1)
        self.kb = KillBroadcaster(id='station_hold', description='Resets Pose')
        # rospy.wait_for_service('/change_wrench')
        self.wrench_changer = rospy.ServiceProxy('/change_wrench',
                                                 WrenchSelect)

        rospy.Subscriber("joy", Joy, self.joy_cb)
        self.reset()

    def reset(self):
        '''
        Used to reset the state of the controller.
        Sometimes when it disconnects then comes back online, the settings are all
            out of wack.
        '''
        self.last_change_mode = False
        self.last_station_hold_state = False
        self.last_kill = False
        self.last_rc = False
        self.last_auto = False

        self.start_count = 0
        self.last_joy = None
        self.active = False

        self.killed = False
        # self.docking = False

        wrench = WrenchStamped()
        wrench.header.frame_id = "/base_link"
        wrench.wrench.force.x = 0
        wrench.wrench.force.y = 0
        wrench.wrench.torque.z = 0
        self.wrench_pub.publish(wrench)

    def check_for_timeout(self, joy):
        if self.last_joy is None:
            self.last_joy = joy
            return

        if joy.axes == self.last_joy.axes and \
           joy.buttons == self.last_joy.buttons:
            # No change in state
            if rospy.Time.now() - self.last_joy.header.stamp > rospy.Duration(
                    15 * 60):
                # The controller times out after 15 minutes
                if self.active:
                    rospy.logwarn(
                        "Controller Timed out. Hold start to resume.")
                    self.reset()
        else:
            joy.header.stamp = rospy.Time.now(
            )  # In the sim, stamps weren't working right
            self.last_joy = joy

    def joy_cb(self, joy):
        self.check_for_timeout(joy)

        # Handle Button presses
        start = joy.buttons[7]
        change_mode = bool(joy.buttons[3])  # Y
        kill = bool(joy.buttons[2])  # X
        station_hold = bool(joy.buttons[0])  # A
        # docking = bool(joy.buttons[1])  # B
        rc_control = bool(joy.buttons[11])  # d-pad left
        auto_control = bool(joy.buttons[12])  # d-pad right

        # Reset controller state if only start is pressed down for awhile
        self.start_count += start
        if self.start_count > 10:  # About 3 seconds
            rospy.loginfo("Resetting controller state.")
            self.reset()
            self.active = True

            self.kill_alarm.clear_alarm()
            self.wrench_changer("rc")

        if not self.active:
            return

        # Change vehicle mode
        if change_mode == 1 and change_mode != self.last_change_mode:
            mode = next(self.wrench_choices)
            rospy.loginfo("Changing Control Mode: {}".format(mode))
            self.wrench_changer(mode)

        if rc_control == 1 and rc_control != self.last_rc:
            rospy.loginfo("Changing Control to RC")
            self.wrench_changer("rc")

        if auto_control == 1 and auto_control != self.last_auto:
            rospy.loginfo("Changing Control to Autonomous")
            self.wrench_changer("autonomous")

        # Station hold
        if station_hold == 1 and station_hold != self.last_station_hold_state:
            rospy.loginfo("Station Holding")
            self.kb.send(active=True)
            self.kb.send(
                active=False)  # Resets c3, this'll change when c3 is replaced
            self.wrench_changer("autonomous")

        # Turn on full system kill
        if kill == 1 and kill != self.last_kill:
            rospy.loginfo("Toggling Kill")
            if self.killed:
                self.kill_alarm.clear_alarm()
            else:
                self.wrench_changer("rc")
                self.kill_alarm.raise_alarm(
                    problem_description='System kill from location: joystick')

            self.killed = not self.killed

        # # Turn on docking mode
        # if docking == 1 and docking != self.last_docking_state:
        #     rospy.loginfo("Toggling Docking")

        #     if self.docking:
        #         self.docking_alarm.clear_alarm()
        #     else:
        #         self.docking_alarm.raise_alarm(
        #             problem_description='Docking kill from location: joystick'
        #         )

        #     self.docking = not self.docking

        self.last_start = start
        self.last_change_mode = change_mode
        self.last_kill = kill
        self.last_station_hold_state = station_hold
        # self.last_docking_state = docking
        self.last_auto_control = auto_control
        self.last_rc = rc_control
        self.last_auto = auto_control

        # Handle joystick commands
        left_stick_x = joy.axes[1]
        left_stick_y = joy.axes[0]
        right_stick_y = joy.axes[3]

        wrench = WrenchStamped()
        wrench.header.frame_id = "/base_link"
        wrench.header.stamp = joy.header.stamp
        wrench.wrench.force.x = self.force_scale * left_stick_x
        wrench.wrench.force.y = self.force_scale * left_stick_y
        wrench.wrench.torque.z = self.torque_scale * right_stick_y
        self.wrench_pub.publish(wrench)
예제 #4
0
class Joystick(object):
    # Base class for whatever you are writing
    def __init__(self):

        self.force_scale = rospy.get_param("~force_scale")
        self.torque_scale = rospy.get_param("~torque_scale")

        self.last_change_mode = False
        self.last_station_hold_state = False
        self.last_kill = False
        self.last_rc = False
        self.last_auto = False

        self.killed = False
        self.docking = False

        self.wrench_choices = itertools.cycle(["rc", "autonomous"])

        self.current_pose = Odometry()

        self.alarm_broadcaster = AlarmBroadcaster()

        self.kill_alarm = self.alarm_broadcaster.add_alarm(name="kill", action_required=True, severity=0)

        self.docking_alarm = self.alarm_broadcaster.add_alarm(name="docking", action_required=True, severity=0)

        self.wrench_pub = rospy.Publisher("/wrench/rc", WrenchStamped, queue_size=1)
        self.kb = KillBroadcaster(id="station_hold", description="Reset Pose")
        # rospy.wait_for_service('/change_wrench')
        self.wrench_changer = rospy.ServiceProxy("/change_wrench", WrenchSelect)

        rospy.Subscriber("joy", Joy, self.joy_cb)
        rospy.Subscriber("odom", Odometry, self.odom_cb)

    def odom_cb(self, msg):
        self.current_pose = msg

    def joy_cb(self, joy):

        # Handle Button presses
        change_mode = bool(joy.buttons[3])  # Y
        kill = bool(joy.buttons[2])  # X
        station_hold = bool(joy.buttons[0])  # A
        docking = bool(joy.buttons[1])  # B
        rc_control = bool(joy.buttons[11])  # d-pad left
        auto_control = bool(joy.buttons[12])  # d-pad right

        # Change vehicle mode
        if change_mode == 1 and change_mode != self.last_change_mode:
            mode = next(self.wrench_choices)
            rospy.loginfo("Changing Control Mode: {}".format(mode))
            self.wrench_changer(mode)

        if rc_control == 1 and rc_control != self.last_rc:
            rospy.loginfo("Changing Control to RC")
            self.wrench_changer("rc")

        if auto_control == 1 and auto_control != self.last_auto:
            rospy.loginfo("Changing Control to Autonomous")
            self.wrench_changer("autonomous")

        # Station hold
        if station_hold == 1 and station_hold != self.last_station_hold_state:
            rospy.loginfo("Station Holding")
            self.kb.send(active=True)
            self.kb.send(active=False)  # Resets c3, this'll change when c3 is replaced
            self.wrench_changer("autonomous")

        # Turn on full system kill
        if kill == 1 and kill != self.last_kill:
            rospy.loginfo("Toggling Kill")
            if self.killed:
                self.kill_alarm.clear_alarm()
            else:
                self.wrench_changer("rc")
                self.kill_alarm.raise_alarm(problem_description="System kill from location: joystick")

            self.killed = not self.killed

        # Turn on docking mode
        if docking == 1 and docking != self.last_docking_state:
            rospy.loginfo("Toggling Docking")

            if self.docking:
                self.docking_alarm.clear_alarm()
            else:
                self.docking_alarm.raise_alarm(problem_description="Docking kill from location: joystick")

            self.docking = not self.docking

        self.last_change_mode = change_mode
        self.last_kill = kill
        self.last_station_hold_state = station_hold
        self.last_docking_state = docking
        self.last_auto_control = auto_control
        self.last_rc = rc_control
        self.last_auto = auto_control

        # Handle joystick commands
        left_stick_x = joy.axes[1]
        left_stick_y = joy.axes[0]
        right_stick_y = joy.axes[3]

        wrench = WrenchStamped()
        wrench.header.frame_id = "/base_link"
        wrench.wrench.force.x = self.force_scale * left_stick_x
        wrench.wrench.force.y = self.force_scale * left_stick_y
        wrench.wrench.torque.z = self.torque_scale * right_stick_y
        self.wrench_pub.publish(wrench)
예제 #5
0
	kill_switch_pub = rospy.Publisher('kill_switch', String, queue_size = 10)	
	
	while not rospy.is_shutdown():

		global status

		status = ReadTerminal()

		status = str(status)

		if 'O' in status:

			msg = "OK"

			rospy.loginfo(msg)

			kill_switch_pub.publish(msg)

		else:

			kill_broadcaster.send(True)	

			msg = "KILL"

			rospy.loginfo(msg)

			kill_switch_pub.publish(msg)

	rospy.spin()	 	
예제 #6
0
    begin = rospy.Publisher("begin", Bool, queue_size = 1) 
    last_pressed = ''
    killed = True
    # configure the serial connections (the parameters differs on the device you are connecting to)
    ser = serial.Serial(
        port='/dev/serial/by-id/usb-Black_Sphere_Technologies_CDC-ACM_Demo_DEMO-if00',
        baudrate=9600,
        #parity=serial.PARITY_ODD,
        #stopbits=serial.STOPBITS_TWO,
        #bytesize=serial.SEVENBITS
    )

    ser.close()
    ser.open()
    ser.isOpen()
    kill_broadcaster.send(killed)
    ser.write('B')
    print 'Enter your commands below.\r\nInsert "exit" to leave the application.'

    input=1
    while not rospy.is_shutdown() :
            # send the character to the device
            # (note that I happend a \r\n carriage return and line feed to the characters - this is requested by my device)
        ser.write('?')
        out = ''
        # let's wait one second before reading output (let's give device time to answer)
        while ser.inWaiting() > 0:
            out += ser.read(1)
        if out == str(4) and str(out) != last_pressed:
            print last_pressed
            if killed == False:
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>')
예제 #8
0
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()
예제 #9
0
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>')