class HACMSWindow(QMainWindow):
def __init__(self):
super(HACMSWindow, self).__init__()
def init_window(self):
self.ui.setupUi(self)
self.init_data_structs()
self.init_widgets()
def init_data_structs(self):
self.trimIncrement = 0.001
self.windowSize = 300
self.in_Base = deque(maxlen=self.windowSize)
self.in_Ref = deque(maxlen=self.windowSize)
self.out_Odom = deque(maxlen=self.windowSize)
self.out_EncL = deque(maxlen=self.windowSize)
self.out_EncR = deque(maxlen=self.windowSize)
self.out_GPS = deque(maxlen=self.windowSize)
def init_widgets(self):
self.remote = Remote(self.ui.console)
self.widgets = [
self.ui.tabWidget,
self.ui.ccButton,
self.ui.rcButton,
self.ui.attackButton,
self.ui.attackComboBox,
self.ui.gpsCheckBox,
self.ui.enclCheckBox,
self.ui.encrCheckBox,
self.ui.saveButton,
self.ui.desiredSpeedLabel,
self.ui.desiredSpeedEdit,
self.ui.setSpeedButton,
self.ui.kpLabel,
self.ui.kpEdit,
self.ui.setKPButton,
self.ui.kiLabel,
self.ui.kiEdit,
self.ui.setKIButton,
self.ui.trimLabel,
self.ui.trimValueLabel,
self.ui.setTrimLeftButton,
self.ui.setTrimRightButton,
self.ui.actualSpeedLabel,
self.ui.actualSpeedLCD,
self.ui.estimatedSpeedLabel,
self.ui.estimatedSpeedLCD,
self.ui.outputPlot,
self.ui.outputPlotLabel,
self.ui.inputPlot,
self.ui.inputPlotLabel,
self.ui.rightPlot,
self.ui.rightPlotLabel
]
#self.ui.mapView.setScene(MapnikScene(self.ui.mapView))
#self.ui.mapView = MapView(self.ui.mapWidget)
self.init_signals()
self.init_plots()
#self.init_waypoints()
def init_signals(self):
self.ui.actionAbout.triggered.connect(self.about)
self.ui.actionQuit.triggered.connect(self.fileQuit)
self.ui.landsharkButton.toggled.connect(self.landshark)
self.ui.ccButton.toggled.connect(self.cc)
self.ui.rcButton.toggled.connect(self.rc)
self.ui.attackButton.toggled.connect(self.attack)
self.ui.attackComboBox.currentIndexChanged.connect(self.attackMode)
self.ui.gpsCheckBox.toggled.connect(self.attackSensor)
self.ui.enclCheckBox.toggled.connect(self.attackSensor)
self.ui.encrCheckBox.toggled.connect(self.attackSensor)
self.ui.saveButton.toggled.connect(self.saveData)
self.ui.setSpeedButton.clicked.connect(self.setLandsharkSpeed)
self.ui.setKPButton.clicked.connect(self.setKP)
self.ui.setKIButton.clicked.connect(self.setKI)
self.ui.setTrimLeftButton.clicked.connect(self.setTrimLeft)
self.ui.setTrimRightButton.clicked.connect(self.setTrimRight)
# Set Validator for parameter fields
self.validator = QDoubleValidator()
self.validator.setNotation(QDoubleValidator.StandardNotation)
self.ui.desiredSpeedEdit.setValidator(self.validator)
self.ui.kpEdit.setValidator(self.validator)
self.ui.kiEdit.setValidator(self.validator)
def init_plots(self):
self.ui.inputPlot.disableAutoRange(pg.ViewBox.YAxis)
self.ui.inputPlot.setYRange(0, 1.3, 0)
self.ui.inputPlot.setBackground('w')
self.ui.inputPlot.hideButtons()
self.ui.inputPlot.showGrid(False, True)
#self.ui.inputPlot.addLegend()
#self.ui.inputPlot.setLabel('left', 'speed')
self.ui.inputPlot.setLabel('top', ' ')
self.ui.inputPlot.setLabel('right', ' ')
self.ui.inputPlot.setLabel('bottom', 'time')
#self.ui.inputPlot.setTitle('Input')
self.inputPlotBase = self.ui.inputPlot.plot(self.in_Base, name='CMD')
self.inputPlotBase.setPen(pg.mkPen(width=3, color='r'))
self.inputPlotRef = self.ui.inputPlot.plot(self.in_Ref, name='REF')
self.inputPlotRef.setPen(pg.mkPen(width=3, color='c'))
self.inputPlotTimer = QTimer()
self.inputPlotTimer.timeout.connect(self.updateInputPlot)
self.ui.outputPlot.setBackground('w')
self.ui.outputPlot.hideButtons()
self.ui.outputPlot.showGrid(False, True)
#self.ui.outputPlot.addLegend()
#self.ui.outputPlot.setLabel('left', 'speed')
self.ui.outputPlot.setLabel('top', ' ')
self.ui.outputPlot.setLabel('right', ' ')
self.ui.outputPlot.setLabel('bottom', 'time')
#self.ui.outputPlot.setTitle('Output')
self.outputPlotGPS = self.ui.outputPlot.plot(self.out_GPS, name='GPS')
self.outputPlotGPS.setPen(pg.mkPen(width=3, color='m'))
self.outputPlotLE = self.ui.outputPlot.plot(self.out_EncL, name='ENC LEFT')
self.outputPlotLE.setPen(pg.mkPen(width=3, color='b'))
self.outputPlotRE = self.ui.outputPlot.plot(self.out_EncR, name='ENC RIGHT')
self.outputPlotRE.setPen(pg.mkPen(width=3, color='g'))
self.outputPlotTimer = QTimer()
self.outputPlotTimer.timeout.connect(self.updateOutputPlot)
self.ui.rightPlot.disableAutoRange(pg.ViewBox.YAxis)
self.ui.rightPlot.setYRange(0, 1.3, 0)
self.ui.rightPlot.setBackground('w')
self.ui.rightPlot.hideButtons()
self.ui.rightPlot.showGrid(False, True)
#self.ui.rightPlot.addLegend()
#self.ui.rightPlot.setLabel('left', 'speed')
self.ui.rightPlot.setLabel('top', ' ')
self.ui.rightPlot.setLabel('right', ' ')
self.ui.rightPlot.setLabel('bottom', 'time')
#self.ui.rightPlot.setTitle('Odometry')
self.rightPlotOdom = self.ui.rightPlot.plot(self.out_Odom, name='SPEED')
self.rightPlotOdom.setPen(pg.mkPen(width=3, color='b'))
self.rightPlotRef = self.ui.rightPlot.plot(self.in_Ref, name='REF')
self.rightPlotRef.setPen(pg.mkPen(width=3, color='c'))
self.rightPlotTimer = QTimer()
self.rightPlotTimer.timeout.connect(self.updateRightPlot)
def startPlotTimers(self):
timerMsec = 500
self.inputPlotTimer.start(timerMsec)
self.outputPlotTimer.start(timerMsec)
self.rightPlotTimer.start(timerMsec)
def stopPlotTimers(self):
self.inputPlotTimer.stop()
self.outputPlotTimer.stop()
self.rightPlotTimer.stop()
def init_waypoints(self):
self.waypointList = QStringList()
self.waypointList.append("test")
self.waypointModel = QStringListModel()
self.ui.waypointListView.setModel(self.waypointModel)
self.waypointModel.setStringList(self.waypointList)
def about(self):
about = QDialog()
about.ui = ui.about_ui.Ui_Dialog()
about.ui.setupUi(about)
about.exec_()
def fileQuit(self):
if self.ui.landsharkButton.isChecked():
self.stop_landshark_comm()
self.close()
def closeEvent(self, ce):
self.fileQuit()
def zeroData(self):
self.in_Base.clear()
self.in_Ref.clear()
self.out_Odom.clear()
self.out_EncL.clear()
self.out_EncR.clear()
self.out_GPS.clear()
def enableAllElements(self):
for widget in self.widgets:
widget.setEnabled(True)
self.zeroData()
def disableAllElements(self):
self.cc(False)
self.rc(False)
self.attack(False)
self.saveData(False)
for widget in self.widgets:
widget.setEnabled(False)
def landshark(self, checked):
if checked:
res = self.remote.startLandshark()
self.start_landshark_comm()
self.enableAllElements()
self.startPlotTimers()
else:
self.stopPlotTimers()
self.disableAllElements()
self.stop_landshark_comm()
res = self.remote.stopLandshark()
self.ui.landsharkButton.setChecked(res)
def cc(self, checked):
if checked:
res = self.remote.startCC()
self.zeroData()
else:
self.saveData(False)
self.rc(False)
self.attack(False)
res = self.remote.stopCC()
self.ui.ccButton.setChecked(res)
def saveData(self, checked):
if checked:
res = self.remote.startSaveData()
else:
res = self.remote.stopSaveData()
self.ui.saveButton.setChecked(res)
def rc(self, checked):
if checked:
try:
self.run_rc_pub.publish(Int32(1))
except:
self.ui.rcButton.setChecked(False)
return
else:
try:
self.run_rc_pub.publish(Int32(0))
except:
self.ui.rcButton.setChecked(True)
return
# For when the button is set via direct method call, not by event call
self.ui.rcButton.setChecked(checked)
def attack(self, checked):
if checked:
try:
self.run_attack_pub.publish(Int32(self.ui.attackComboBox.currentIndex()+1))
self.sensor_attack_pub.publish(Int32(self.getAttackSensorValue()))
except:
self.ui.attackButton.setChecked(False)
return
else:
try:
self.run_attack_pub.publish(Int32(0))
except:
self.ui.attackButton.setChecked(True)
return
# For when the button is set via direct method call, not by event call
self.ui.attackButton.setChecked(checked)
# Called by attackMode combobox when there is a change in the selected index.
def attackMode(self, index):
if self.ui.attackButton.isChecked():
try:
self.run_attack_pub.publish(Int32(index+1)) # Start base index at 1
except:
return
# Called by sensor checkboxes when there is a check or uncheck event.
def attackSensor(self):
if self.ui.attackButton.isChecked():
try:
self.sensor_attack_pub.publish(Int32(self.getAttackSensorValue()))
except:
return
# Determine the appropriate binary-style representation for which sensor checkboxes are checked.
def getAttackSensorValue(self):
value = 0
if self.ui.gpsCheckBox.isChecked():
value += 4
if self.ui.enclCheckBox.isChecked():
value += 2
if self.ui.encrCheckBox.isChecked():
value += 1
return value
def getWidgetColor(self, widget):
style = widget.styleSheet()
if "background-color: green;" in style:
return "green"
if "background-color: red;" in style:
return "red"
def toggleWidgetColor(self, widget, setColor=None):
style = widget.styleSheet()
if setColor is None:
if self.getWidgetColor(widget) is "green":
widget.setStyleSheet(string.replace(style, "background-color: green;", "background-color: red;"))
elif self.getWidgetColor(widget) is "red":
widget.setStyleSheet(string.replace(style, "background-color: red;", "background-color: green;"))
else:
if setColor is "red":
widget.setStyleSheet(string.replace(style, "background-color: green;", "background-color: red;"))
elif setColor is "green":
widget.setStyleSheet(string.replace(style, "background-color: red;", "background-color: green;"))
def updateActualSpeedLCD(self, value):
self.ui.actualSpeedLCD.display(value)
def updateEstimatedSpeedLCD(self, value):
self.ui.estimatedSpeedLCD.display(value)
def updateInputPlot(self):
""" Redraws the input plot
"""
self.inputPlotBase.setData(self.in_Base)
self.inputPlotRef.setData(self.in_Ref)
def updateOutputPlot(self):
""" Redraws the output plot
"""
self.outputPlotGPS.setData(self.out_GPS)
self.outputPlotLE.setData(self.out_EncL)
self.outputPlotRE.setData(self.out_EncR)
def updateRightPlot(self):
""" Redraws the righthand plot
"""
self.rightPlotOdom.setData(self.out_Odom)
self.rightPlotRef.setData(self.in_Ref)
def setLandsharkSpeed(self):
self.desired_speed_pub.publish(Float32(float(self.ui.desiredSpeedEdit.text())))
def setKP(self):
self.kp_pub.publish(Float32(float(self.ui.kpEdit.text())))
def setKI(self):
self.ki_pub.publish(Float32(float(self.ui.kiEdit.text())))
def setTrimLeft(self):
# Get current trim value
trim = float(self.ui.trimValueLabel.text())
# Decrement trim value
trim -= self.trimIncrement
# Display updated trim value
self.ui.trimValueLabel.setText(str(trim))
# Publish new trim value
self.trim_pub.publish(Float32(trim))
def setTrimRight(self):
# Get current trim value
trim = float(self.ui.trimValueLabel.text())
# Increment trim value
trim += self.trimIncrement
# Display updated trim value
self.ui.trimValueLabel.setText(str(trim))
# Publish new trim value
self.trim_pub.publish(Float32(trim))
def start_landshark_comm(self):
# Initialize ROS node
rospy.init_node('landshark_demo', disable_signals=True)
# Subscribe to HACMS Demo topics
self.base_sub = rospy.Subscriber("/landshark_demo/base_vel", TwistStamped, self.captureBase)
self.ref_sub = rospy.Subscriber("/landshark_demo/ref_vel", TwistStamped, self.captureRef)
self.est_sub = rospy.Subscriber("/landshark_demo/est_vel", TwistStamped, self.captureEst)
self.odom_sub = rospy.Subscriber("/landshark_demo/odom", Odometry, self.captureOdom)
self.encL_sub = rospy.Subscriber("/landshark_demo/left_enc_vel", TwistStamped, self.captureEncL)
self.encR_sub = rospy.Subscriber("/landshark_demo/right_enc_vel", TwistStamped, self.captureEncR)
self.gps_sub = rospy.Subscriber("/landshark_demo/gps_vel", TwistStamped, self.captureGPS)
# Publish to HACMS Demo topics
self.desired_speed_pub = rospy.Publisher('/landshark_demo/desired_speed', Float32)
self.trim_pub = rospy.Publisher('/landshark_demo/trim', Float32)
self.kp_pub = rospy.Publisher('/landshark_demo/kp', Float32)
self.ki_pub = rospy.Publisher('/landshark_demo/ki', Float32)
self.run_rc_pub = rospy.Publisher('/landshark_demo/run_rc', Int32)
self.run_attack_pub = rospy.Publisher('/landshark_demo/run_attack', Int32)
self.sensor_attack_pub = rospy.Publisher('/landshark_demo/sensor_attack', Int32)
return True
def stop_landshark_comm(self):
# Unregister HACMS Demo subscribed topics
self.base_sub.unregister()
self.ref_sub.unregister()
self.est_sub.unregister()
self.odom_sub.unregister()
self.encL_sub.unregister()
self.encR_sub.unregister()
self.gps_sub.unregister()
# Unregister HACMS Demo published topics
self.desired_speed_pub.unregister()
self.trim_pub.unregister()
self.kp_pub.unregister()
self.ki_pub.unregister()
self.run_rc_pub.unregister()
self.run_attack_pub.unregister()
self.sensor_attack_pub.unregister()
#rospy.signal_shutdown("Turning off ROSPy") TODO - How do we restart ROSPy
# Keep in mind ROSPy doesn't properly start back up when this is uncommented.
return True
def captureBase(self, msg):
self.in_Base.append(msg.twist.linear.x)
def captureRef(self, msg):
self.in_Ref.append(msg.twist.linear.x)
def captureEst(self, msg):
self.updateEstimatedSpeedLCD(msg.twist.linear.x)
def captureOdom(self, msg):
self.updateActualSpeedLCD(msg.twist.twist.linear.x)
self.out_Odom.append(msg.twist.twist.linear.x)
def captureEncL(self, msg):
self.out_EncL.append(msg.twist.linear.x)
def captureEncR(self, msg):
self.out_EncR.append(msg.twist.linear.x)
def captureGPS(self, msg):
self.out_GPS.append(msg.twist.linear.x)
return
class HACMSWindow(QMainWindow):
def __init__(self):
super(HACMSWindow, self).__init__()
def init_window(self):
self.ui.setupUi(self)
self.init_data_structs()
self.init_widgets()
def init_data_structs(self):
self.trimIncrement = 0.001
self.windowSize = 300
self.in_Base = deque(maxlen=self.windowSize)
self.in_Ref = deque(maxlen=self.windowSize)
self.out_Odom = deque(maxlen=self.windowSize)
self.out_EncL = deque(maxlen=self.windowSize)
self.out_EncR = deque(maxlen=self.windowSize)
self.out_GPS = deque(maxlen=self.windowSize)
def init_widgets(self):
self.remote = Remote(self.ui.console)
self.widgets = [
self.ui.tabWidget, self.ui.ccButton, self.ui.rcButton,
self.ui.attackButton, self.ui.attackComboBox, self.ui.gpsCheckBox,
self.ui.enclCheckBox, self.ui.encrCheckBox, self.ui.saveButton,
self.ui.desiredSpeedLabel, self.ui.desiredSpeedEdit,
self.ui.setSpeedButton, self.ui.kpLabel, self.ui.kpEdit,
self.ui.setKPButton, self.ui.kiLabel, self.ui.kiEdit,
self.ui.setKIButton, self.ui.trimLabel, self.ui.trimValueLabel,
self.ui.setTrimLeftButton, self.ui.setTrimRightButton,
self.ui.actualSpeedLabel, self.ui.actualSpeedLCD,
self.ui.estimatedSpeedLabel, self.ui.estimatedSpeedLCD,
self.ui.outputPlot, self.ui.outputPlotLabel, self.ui.inputPlot,
self.ui.inputPlotLabel, self.ui.rightPlot, self.ui.rightPlotLabel
]
#self.ui.mapView.setScene(MapnikScene(self.ui.mapView))
#self.ui.mapView = MapView(self.ui.mapWidget)
self.init_signals()
self.init_plots()
#self.init_waypoints()
def init_signals(self):
self.ui.actionAbout.triggered.connect(self.about)
self.ui.actionQuit.triggered.connect(self.fileQuit)
self.ui.landsharkButton.toggled.connect(self.landshark)
self.ui.ccButton.toggled.connect(self.cc)
self.ui.rcButton.toggled.connect(self.rc)
self.ui.attackButton.toggled.connect(self.attack)
self.ui.attackComboBox.currentIndexChanged.connect(self.attackMode)
self.ui.gpsCheckBox.toggled.connect(self.attackSensor)
self.ui.enclCheckBox.toggled.connect(self.attackSensor)
self.ui.encrCheckBox.toggled.connect(self.attackSensor)
self.ui.saveButton.toggled.connect(self.saveData)
self.ui.setSpeedButton.clicked.connect(self.setLandsharkSpeed)
self.ui.setKPButton.clicked.connect(self.setKP)
self.ui.setKIButton.clicked.connect(self.setKI)
self.ui.setTrimLeftButton.clicked.connect(self.setTrimLeft)
self.ui.setTrimRightButton.clicked.connect(self.setTrimRight)
# Set Validator for parameter fields
self.validator = QDoubleValidator()
self.validator.setNotation(QDoubleValidator.StandardNotation)
self.ui.desiredSpeedEdit.setValidator(self.validator)
self.ui.kpEdit.setValidator(self.validator)
self.ui.kiEdit.setValidator(self.validator)
def init_plots(self):
self.ui.inputPlot.disableAutoRange(pg.ViewBox.YAxis)
self.ui.inputPlot.setYRange(0, 1.3, 0)
self.ui.inputPlot.setBackground('w')
self.ui.inputPlot.hideButtons()
self.ui.inputPlot.showGrid(False, True)
#self.ui.inputPlot.addLegend()
#self.ui.inputPlot.setLabel('left', 'speed')
self.ui.inputPlot.setLabel('top', ' ')
self.ui.inputPlot.setLabel('right', ' ')
self.ui.inputPlot.setLabel('bottom', 'time')
#self.ui.inputPlot.setTitle('Input')
self.inputPlotBase = self.ui.inputPlot.plot(self.in_Base, name='CMD')
self.inputPlotBase.setPen(pg.mkPen(width=3, color='r'))
self.inputPlotRef = self.ui.inputPlot.plot(self.in_Ref, name='REF')
self.inputPlotRef.setPen(pg.mkPen(width=3, color='c'))
self.inputPlotTimer = QTimer()
self.inputPlotTimer.timeout.connect(self.updateInputPlot)
self.ui.outputPlot.setBackground('w')
self.ui.outputPlot.hideButtons()
self.ui.outputPlot.showGrid(False, True)
#self.ui.outputPlot.addLegend()
#self.ui.outputPlot.setLabel('left', 'speed')
self.ui.outputPlot.setLabel('top', ' ')
self.ui.outputPlot.setLabel('right', ' ')
self.ui.outputPlot.setLabel('bottom', 'time')
#self.ui.outputPlot.setTitle('Output')
self.outputPlotGPS = self.ui.outputPlot.plot(self.out_GPS, name='GPS')
self.outputPlotGPS.setPen(pg.mkPen(width=3, color='m'))
self.outputPlotLE = self.ui.outputPlot.plot(self.out_EncL,
name='ENC LEFT')
self.outputPlotLE.setPen(pg.mkPen(width=3, color='b'))
self.outputPlotRE = self.ui.outputPlot.plot(self.out_EncR,
name='ENC RIGHT')
self.outputPlotRE.setPen(pg.mkPen(width=3, color='g'))
self.outputPlotTimer = QTimer()
self.outputPlotTimer.timeout.connect(self.updateOutputPlot)
self.ui.rightPlot.disableAutoRange(pg.ViewBox.YAxis)
self.ui.rightPlot.setYRange(0, 1.3, 0)
self.ui.rightPlot.setBackground('w')
self.ui.rightPlot.hideButtons()
self.ui.rightPlot.showGrid(False, True)
#self.ui.rightPlot.addLegend()
#self.ui.rightPlot.setLabel('left', 'speed')
self.ui.rightPlot.setLabel('top', ' ')
self.ui.rightPlot.setLabel('right', ' ')
self.ui.rightPlot.setLabel('bottom', 'time')
#self.ui.rightPlot.setTitle('Odometry')
self.rightPlotOdom = self.ui.rightPlot.plot(self.out_Odom,
name='SPEED')
self.rightPlotOdom.setPen(pg.mkPen(width=3, color='b'))
self.rightPlotRef = self.ui.rightPlot.plot(self.in_Ref, name='REF')
self.rightPlotRef.setPen(pg.mkPen(width=3, color='c'))
self.rightPlotTimer = QTimer()
self.rightPlotTimer.timeout.connect(self.updateRightPlot)
def startPlotTimers(self):
timerMsec = 500
self.inputPlotTimer.start(timerMsec)
self.outputPlotTimer.start(timerMsec)
self.rightPlotTimer.start(timerMsec)
def stopPlotTimers(self):
self.inputPlotTimer.stop()
self.outputPlotTimer.stop()
self.rightPlotTimer.stop()
def init_waypoints(self):
self.waypointList = QStringList()
self.waypointList.append("test")
self.waypointModel = QStringListModel()
self.ui.waypointListView.setModel(self.waypointModel)
self.waypointModel.setStringList(self.waypointList)
def about(self):
about = QDialog()
about.ui = ui.about_ui.Ui_Dialog()
about.ui.setupUi(about)
about.exec_()
def fileQuit(self):
if self.ui.landsharkButton.isChecked():
self.stop_landshark_comm()
self.close()
def closeEvent(self, ce):
self.fileQuit()
def zeroData(self):
self.in_Base.clear()
self.in_Ref.clear()
self.out_Odom.clear()
self.out_EncL.clear()
self.out_EncR.clear()
self.out_GPS.clear()
def enableAllElements(self):
for widget in self.widgets:
widget.setEnabled(True)
self.zeroData()
def disableAllElements(self):
self.cc(False)
self.rc(False)
self.attack(False)
self.saveData(False)
for widget in self.widgets:
widget.setEnabled(False)
def landshark(self, checked):
if checked:
res = self.remote.startLandshark()
self.start_landshark_comm()
self.enableAllElements()
self.startPlotTimers()
else:
self.stopPlotTimers()
self.disableAllElements()
self.stop_landshark_comm()
res = self.remote.stopLandshark()
self.ui.landsharkButton.setChecked(res)
def cc(self, checked):
if checked:
res = self.remote.startCC()
self.zeroData()
else:
self.saveData(False)
self.rc(False)
self.attack(False)
res = self.remote.stopCC()
self.ui.ccButton.setChecked(res)
def saveData(self, checked):
if checked:
res = self.remote.startSaveData()
else:
res = self.remote.stopSaveData()
self.ui.saveButton.setChecked(res)
def rc(self, checked):
if checked:
try:
self.run_rc_pub.publish(Int32(1))
except:
self.ui.rcButton.setChecked(False)
return
else:
try:
self.run_rc_pub.publish(Int32(0))
except:
self.ui.rcButton.setChecked(True)
return
# For when the button is set via direct method call, not by event call
self.ui.rcButton.setChecked(checked)
def attack(self, checked):
if checked:
try:
self.run_attack_pub.publish(
Int32(self.ui.attackComboBox.currentIndex() + 1))
self.sensor_attack_pub.publish(
Int32(self.getAttackSensorValue()))
except:
self.ui.attackButton.setChecked(False)
return
else:
try:
self.run_attack_pub.publish(Int32(0))
except:
self.ui.attackButton.setChecked(True)
return
# For when the button is set via direct method call, not by event call
self.ui.attackButton.setChecked(checked)
# Called by attackMode combobox when there is a change in the selected index.
def attackMode(self, index):
if self.ui.attackButton.isChecked():
try:
self.run_attack_pub.publish(Int32(index +
1)) # Start base index at 1
except:
return
# Called by sensor checkboxes when there is a check or uncheck event.
def attackSensor(self):
if self.ui.attackButton.isChecked():
try:
self.sensor_attack_pub.publish(
Int32(self.getAttackSensorValue()))
except:
return
# Determine the appropriate binary-style representation for which sensor checkboxes are checked.
def getAttackSensorValue(self):
value = 0
if self.ui.gpsCheckBox.isChecked():
value += 4
if self.ui.enclCheckBox.isChecked():
value += 2
if self.ui.encrCheckBox.isChecked():
value += 1
return value
def getWidgetColor(self, widget):
style = widget.styleSheet()
if "background-color: green;" in style:
return "green"
if "background-color: red;" in style:
return "red"
def toggleWidgetColor(self, widget, setColor=None):
style = widget.styleSheet()
if setColor is None:
if self.getWidgetColor(widget) is "green":
widget.setStyleSheet(
string.replace(style, "background-color: green;",
"background-color: red;"))
elif self.getWidgetColor(widget) is "red":
widget.setStyleSheet(
string.replace(style, "background-color: red;",
"background-color: green;"))
else:
if setColor is "red":
widget.setStyleSheet(
string.replace(style, "background-color: green;",
"background-color: red;"))
elif setColor is "green":
widget.setStyleSheet(
string.replace(style, "background-color: red;",
"background-color: green;"))
def updateActualSpeedLCD(self, value):
self.ui.actualSpeedLCD.display(value)
def updateEstimatedSpeedLCD(self, value):
self.ui.estimatedSpeedLCD.display(value)
def updateInputPlot(self):
""" Redraws the input plot
"""
self.inputPlotBase.setData(self.in_Base)
self.inputPlotRef.setData(self.in_Ref)
def updateOutputPlot(self):
""" Redraws the output plot
"""
self.outputPlotGPS.setData(self.out_GPS)
self.outputPlotLE.setData(self.out_EncL)
self.outputPlotRE.setData(self.out_EncR)
def updateRightPlot(self):
""" Redraws the righthand plot
"""
self.rightPlotOdom.setData(self.out_Odom)
self.rightPlotRef.setData(self.in_Ref)
def setLandsharkSpeed(self):
self.desired_speed_pub.publish(
Float32(float(self.ui.desiredSpeedEdit.text())))
def setKP(self):
self.kp_pub.publish(Float32(float(self.ui.kpEdit.text())))
def setKI(self):
self.ki_pub.publish(Float32(float(self.ui.kiEdit.text())))
def setTrimLeft(self):
# Get current trim value
trim = float(self.ui.trimValueLabel.text())
# Decrement trim value
trim -= self.trimIncrement
# Display updated trim value
self.ui.trimValueLabel.setText(str(trim))
# Publish new trim value
self.trim_pub.publish(Float32(trim))
def setTrimRight(self):
# Get current trim value
trim = float(self.ui.trimValueLabel.text())
# Increment trim value
trim += self.trimIncrement
# Display updated trim value
self.ui.trimValueLabel.setText(str(trim))
# Publish new trim value
self.trim_pub.publish(Float32(trim))
def start_landshark_comm(self):
# Initialize ROS node
rospy.init_node('landshark_demo', disable_signals=True)
# Subscribe to HACMS Demo topics
self.base_sub = rospy.Subscriber("/landshark_demo/base_vel",
TwistStamped, self.captureBase)
self.ref_sub = rospy.Subscriber("/landshark_demo/ref_vel",
TwistStamped, self.captureRef)
self.est_sub = rospy.Subscriber("/landshark_demo/est_vel",
TwistStamped, self.captureEst)
self.odom_sub = rospy.Subscriber("/landshark_demo/odom", Odometry,
self.captureOdom)
self.encL_sub = rospy.Subscriber("/landshark_demo/left_enc_vel",
TwistStamped, self.captureEncL)
self.encR_sub = rospy.Subscriber("/landshark_demo/right_enc_vel",
TwistStamped, self.captureEncR)
self.gps_sub = rospy.Subscriber("/landshark_demo/gps_vel",
TwistStamped, self.captureGPS)
# Publish to HACMS Demo topics
self.desired_speed_pub = rospy.Publisher(
'/landshark_demo/desired_speed', Float32)
self.trim_pub = rospy.Publisher('/landshark_demo/trim', Float32)
self.kp_pub = rospy.Publisher('/landshark_demo/kp', Float32)
self.ki_pub = rospy.Publisher('/landshark_demo/ki', Float32)
self.run_rc_pub = rospy.Publisher('/landshark_demo/run_rc', Int32)
self.run_attack_pub = rospy.Publisher('/landshark_demo/run_attack',
Int32)
self.sensor_attack_pub = rospy.Publisher(
'/landshark_demo/sensor_attack', Int32)
return True
def stop_landshark_comm(self):
# Unregister HACMS Demo subscribed topics
self.base_sub.unregister()
self.ref_sub.unregister()
self.est_sub.unregister()
self.odom_sub.unregister()
self.encL_sub.unregister()
self.encR_sub.unregister()
self.gps_sub.unregister()
# Unregister HACMS Demo published topics
self.desired_speed_pub.unregister()
self.trim_pub.unregister()
self.kp_pub.unregister()
self.ki_pub.unregister()
self.run_rc_pub.unregister()
self.run_attack_pub.unregister()
self.sensor_attack_pub.unregister()
#rospy.signal_shutdown("Turning off ROSPy") TODO - How do we restart ROSPy
# Keep in mind ROSPy doesn't properly start back up when this is uncommented.
return True
def captureBase(self, msg):
self.in_Base.append(msg.twist.linear.x)
def captureRef(self, msg):
self.in_Ref.append(msg.twist.linear.x)
def captureEst(self, msg):
self.updateEstimatedSpeedLCD(msg.twist.linear.x)
def captureOdom(self, msg):
self.updateActualSpeedLCD(msg.twist.twist.linear.x)
self.out_Odom.append(msg.twist.twist.linear.x)
def captureEncL(self, msg):
self.out_EncL.append(msg.twist.linear.x)
def captureEncR(self, msg):
self.out_EncR.append(msg.twist.linear.x)
def captureGPS(self, msg):
self.out_GPS.append(msg.twist.linear.x)
return
