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)
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)
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)
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)
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()
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>')
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>')