def __init__(self, lc):
self.lc = lc
self.fsm_state_sub = lc.subscribe("STATE", self.handle_fsm_state)
self.pod_se_fb_sub = lc.subscribe("_SE_FB", self.handle_pod_se_fb)
self.tempState = state_t()
self.last_fsm_state = self.tempState.SAFE
# error states
self.do_estop = False
self.last_state_msg_timestamp = -1.
# in case this is useful:
self.last_state_msg_localtime = -1.
# quick and dirty mode switch estimator
self.acceleration_est = 0.
self.velocity_last = 0.
self.last_print_time = time.time()
self.last_publish_timestamp = self.last_state_msg_timestamp
self.last_publish_time = time.time()
def __init__(self, lc):
self.lc = lc
self.brake_config = brake_trajectory_simple_t()
self.brake_config.accel_alpha = 0.1
self.brake_config.accel_bias_alpha = 0.0001
self.brake_config.launch_start_accel_duration = 1.0
self.brake_config.launch_end_accel_duration = 1.0
self.brake_config.launch_start_accel_threshold = 0.4
self.brake_config.launch_end_accel_threshold = 0.4
self.brake_config.end_of_launch_timeout = 10.0
self.brake_config.cruise_setpoint = 5.3
self.brake_config.flight_softstop_timeout = 20.0
self.brake_config.soft_stop_setpoint = 1.0
self.brake_config.brake_trajectory_start_close_time = 0
self.brake_config.brake_trajectory_start_braking_speed = 1000
self.brake_config.brake_trajectory_close_rate_inch_per_second = 100
self.config_pub = lc.subscribe("FSM_CONFIG_SET",
self.handle_config_set)
temp_state = state_t()
self.states = {
"ESTOP": temp_state.ESTOP,
"SHUTDOWN": temp_state.SHUTDOWN,
"SOFT_STOP": temp_state.SOFT_STOP,
"ARMING": temp_state.ARMING,
"ARM": temp_state.ARM,
"LAUNCH": temp_state.LAUNCH,
"FLIGHT": temp_state.FLIGHT,
"PRE_DRIVE": temp_state.PRE_DRIVE,
"DRIVE": temp_state.DRIVE,
"TELEOP": temp_state.TELEOP
}
self.state_name_map = {}
for key in self.states.keys():
self.state_name_map[self.states[key]] = key
self.state = self.states["ESTOP"]
self.requested_state_sub = lc.subscribe("FSM_REQUESTED_STATE",
self.handle_requested_state)
self.estop_sub = lc.subscribe("_ESTOP", self.handle_estop)
self.pod_imu_sub = lc.subscribe("_FC_OH",
self.flight_control_high_rate_handler)
self.last_fc_high_rate_recv_time = None
# quick and dirty mode switch estimator
self.acc_estimator_lock = Lock()
self.acceleration_est = 0.
self.acceleration_bias_est = 0.
self.acceleration_bias_est_sum = 0.
self.taring_counter = 0
self.bac_state_high_rate_sub = lc.subscribe(
"_BAC_STATE_H", self.handle_bac_state_high_rate)
self.last_bac_high_rate_recv_time = None
self.brake_width = 0.
self.brake_pressure = 0.
# some brake traj state
self.doing_braking = False
self.doing_braking_setpoint = 0.0
self.last_brake_trajectory_update_time = time.time() - 1000
self.bac_mode_sub = lc.subscribe("_BAC_MODE_STATE",
self.handle_bac_mode)
self.last_bac_mode_recv_time = None
self.bac_mode = None
self.se_sub = lc.subscribe("_FC_SE", self.handle_state_estimate)
self.last_se_recv_time = None
self.particles = None
self.stopped = None
self.se_lock = Lock()
##self.tare_sub = lc.subscribe("TARE", self.handle_brake_tare)
self.last_print_time = time.time()
self.last_publish_time = time.time()
self.last_estop_publish_time = time.time()
self.last_commanded_setpoint = time.time()
self.last_commanded_mode_switch = time.time()
self.last_publish_fsm_state = time.time()
self.last_requested_to_go_to_estop = time.time()
self.last_published_config = time.time()
self.last_accel_below_threshold = time.time()
self.last_accel_above_threshold = time.time()
def publish_state(self):
msg = state_t()
msg.utime = int(time.time() * 1000 * 1000)
msg.currentState = self.state
self.lc.publish("FSM_STATE", msg.encode())
def publish_estop(self):
msg = state_t()
msg.utime = int(self.last_state_msg_timestamp * 1000 * 1000)
msg.currentState = self.tempState.ESTOP
self.lc.publish("REQUESTED_STATE", msg.encode())
# until cleared through the interface or restarting this relay. STATUS_IDLE = 1 # pod on but not ready to be pushed. STATUS_READY = 2 # Ready to be pushed. When in ARM, we publish this. STATUS_PUSHING = 3 # Pod detects it is being pushed. When in LAUNCH, publish this. STATUS_COAST = 4 # Pod detects it has separated from pusher vehicle. When in FLIGHT, publish this. STATUS_BRAKING = 5 # Pod is applying its brakes. When in FLIGHT and brakes are less open than FULL, publish this. MESSAGE_SEND_PERIOD = 0.0333 WATCHDOG = 2.0 fault_thrown = False last_heard_estop = time.time() state = None stmsg = state_t() last_heard_state = time.time() - WATCHDOG particles = None last_heard_state_estimate = time.time() - WATCHDOG high_power_battery_info = None last_heard_high_battery = time.time() - WATCHDOG pod_temp = None last_heard_pod_temp = time.time() - WATCHDOG stripe_count = None last_heard_fiducial = time.time() - WATCHDOG brake_pos = None
def do_soft_stop(self):
msg = state_t()
msg.utime = time.time() * 1000 * 1000
msg.currentState = msg.SOFT_STOP
self.lc.publish("FSM_REQUESTED_STATE", msg.encode())
def arm_pod(self):
sendTime = time.time()
state_msg = state_t()
state_msg.utime = 0
state_msg.currentState = state_msg.ARM
self.lc.publish("REQUESTED_STATE", state_msg.encode())
def sendPreDrive(self):
msg = state_t()
msg.utime = time.time() * 1000 * 1000
msg.currentState = msg.PRE_DRIVE
self.lc.publish("FSM_REQUESTED_STATE", msg.encode())
def sendTeleop(self):
msg = state_t()
msg.utime = time.time() * 1000 * 1000
msg.currentState = msg.TELEOP
self.lc.publish("FSM_REQUESTED_STATE", msg.encode())
def sendArming(self):
msg = state_t()
msg.utime = time.time() * 1000 * 1000
msg.currentState = msg.ARMING
self.lc.publish("FSM_REQUESTED_STATE", msg.encode())
def sendShutdown(self):
msg = state_t()
msg.utime = time.time() * 1000 * 1000
msg.currentState = msg.SHUTDOWN
self.lc.publish("FSM_REQUESTED_STATE", msg.encode())
self.setLayout(layout)
if __name__ == '__main__':
# hook up interrupt signal
signal.signal(signal.SIGINT, signal.SIG_DFL)
lc = create_lcm()
app = QtGui.QApplication(sys.argv)
overallWidget = QtGui.QWidget()
modecontrol = ModeControlUI(lc)
overallLayout = QtGui.QVBoxLayout(overallWidget)
overallLayout.addWidget(modecontrol)
overallWidget.setWindowTitle('Mode Control Buttons')
overallWidget.show()
overallWidget.resize(600, 300)
# Test Handlers with special values
init_state = state_t()
init_state.currentState = -1
modecontrol.model.new_state_handler('FSM_STATE', init_state.encode())
init_config = bac_config_t()
init_config.minimum_disarm_duration = -1
modecontrol.model.new_config_handler('_BAC_CONFIG_STATE',
init_config.encode())
start_lcm(lc)
sys.exit(app.exec_())
def __init__(self, config, lc=None, parent=None, name=None):
super(PodDriverWidget, self).__init__(parent)
self.startTime = time.time()
self.lc = lc
if name:
self.setObjectName(name)
self.startTime = time.time()
self.currentState = ''
self.stateLabel = QtGui.QLabel(self.currentState)
temp_state = state_t()
self.arm = int(temp_state.ARM)
self.launch = int(temp_state.LAUNCH)
self.flight = int(temp_state.FLIGHT)
self.safe = int(temp_state.SAFE)
self.floating = int(temp_state.FLOATING)
self.drive = int(temp_state.DRIVE)
self.soft_stop = int(temp_state.SOFT_STOP)
self.estop = int(temp_state.ESTOP)
self.fault = int(temp_state.FAULT)
self.critical_states = [self.estop, self.soft_stop, self.fault]
self.caution_states = [
self.arm, self.launch, self.flight, self.floating, self.drive
]
self.safe_states = [self.safe]
self.state_lst = [
self.arm, self.launch, self.flight, self.safe, self.floating,
self.drive, self.soft_stop, self.estop, self.fault
]
self.state_string_lst = [
'ARM', 'LAUNCH', 'FLIGHT', 'SAFE', 'FLOATING', 'DRIVE',
'SOFT_STOP', 'ESTOP', 'FAULT'
]
self.state_dict = dict(zip(self.state_string_lst, self.state_lst))
self.reverse_state_dict = dict(
zip(self.state_lst, self.state_string_lst))
self.stateDropdown = QtGui.QComboBox(self)
for state in self.state_string_lst:
self.stateDropdown.addItem(state)
self.stateDropdown.activated[str].connect(self.publish_state_request)
self.softStopButton = QtGui.QPushButton('SOFT-STOP', self)
self.softStopButton.setStyleSheet(
'background-color: rgba(255, 255, 0, 150); font-size: 18pt;')
self.softStopButton.clicked.connect(self.publish_softstop_request)
self.softStopButton.setSizePolicy(
QtGui.QSizePolicy(QtGui.QSizePolicy.Expanding,
QtGui.QSizePolicy.Expanding))
self.eStopButton = QtGui.QPushButton('E-STOP', self)
self.eStopButton.setSizePolicy(
QtGui.QSizePolicy(QtGui.QSizePolicy.Expanding,
QtGui.QSizePolicy.Expanding))
self.eStopButton.setStyleSheet(
'background-color: rgba(255, 0, 0, 150); font-size: 18pt;')
self.eStopButton.clicked.connect(self.publish_estop_request)
mainLayout = QtGui.QGridLayout()
mainLayout.addWidget(self.softStopButton, 0, 0, 2, 1)
mainLayout.addWidget(self.eStopButton, 0, 1, 2, 1)
mainLayout.addWidget(self.stateLabel, 2, 0, 1, 1)
mainLayout.addWidget(self.stateDropdown, 2, 1, 1, 1)
self.setLayout(mainLayout)
self.timer = QtCore.QTimer()
self.timer.timeout.connect(self.update)
self.timer.start(33)
currentstate_sub = self.lc.subscribe("STATE", self.handle_state_t)
def publish_state_request(self, requested_state):
sendTime = time.time()
state_msg = state_t()
state_msg.utime = int((sendTime - self.startTime) * 1000000)
state_msg.currentState = self.state_dict[str(requested_state)]
self.lc.publish("REQUESTED_STATE", state_msg.encode())
