def __init__(self): self.station_holding = False self.kill_alarm = False self.voltage = None self.wrench = None rospy.Subscriber("/wrench/current", String, self.wrench_current_cb) rospy.Subscriber("/battery_monitor", Float32, self.battery_monitor_cb) self.markers_pub = rospy.Publisher('/boat_info', Marker, queue_size=10) self.kill_listener = AlarmListener('kill', self.kill_alarm_cb) self.station_hold_listner = AlarmListener('station_hold', self.station_alarm_cb)
def connect_ros(self): ''' Connect ROS nodes, services, and alarms to variables and methods within this class. ''' # Attempts to read the battery voltage parameters (sets them to defaults if they have not been set) self.battery_low_voltage = rospy.get_param( "/battery_monitor/battery_low_voltage", 22.1) self.battery_critical_voltage = rospy.get_param( "/battery_monitor/battery_critical_voltage", 20.6) rospy.Subscriber("/battery_monitor", Float32, self.cache_battery_voltage) rospy.Subscriber("/clock", Clock, self.cache_system_time) self.wrench_changer = rospy.ServiceProxy('/change_wrench', WrenchSelect) self.kill_listener = AlarmListener('kill', self.update_kill_status) alarm_broadcaster = AlarmBroadcaster() self.kill_alarm = alarm_broadcaster.add_alarm(name='kill', action_required=True, severity=0) self.station_hold_alarm = alarm_broadcaster.add_alarm( name='station_hold', action_required=True, severity=3)
def __init__(self, controller_name, wrench_pub=None): self.name = controller_name self.wrench_choices = itertools.cycle(["rc", "keyboard", "autonomous"]) self.alarm_broadcaster = AlarmBroadcaster() self.kill_alarm = self.alarm_broadcaster.add_alarm( name="kill", action_required=True, severity=0 ) self.station_hold_alarm = self.alarm_broadcaster.add_alarm( name="station_hold", action_required=False, severity=3 ) # rospy.wait_for_service("/change_wrench") self.wrench_changer = rospy.ServiceProxy("/change_wrench", WrenchSelect) self.kill_listener = AlarmListener("kill", self._update_kill_status) if (wrench_pub is None): self.wrench_pub = wrench_pub else: self.wrench_pub = rospy.Publisher(wrench_pub, WrenchStamped, queue_size=1) self.shooter_load_client = actionlib.SimpleActionClient("/shooter/load", ShooterDoAction) self.shooter_fire_client = actionlib.SimpleActionClient("/shooter/fire", ShooterDoAction) self.shooter_cancel_client = rospy.ServiceProxy("/shooter/cancel", Trigger) self.shooter_manual_client = rospy.ServiceProxy("/shooter/manual", ShooterManual) self.shooter_reset_client = rospy.ServiceProxy("/shooter/reset", Trigger) self.is_killed = False self.is_timed_out = False self.clear_wrench()
def __init__(self): fprint("Starting station hold listener...") self.client = actionlib.SimpleActionClient('/move_to', MoveAction) fprint("Waiting for action server...") self.client.wait_for_server() self.change_wrench = rospy.ServiceProxy("/change_wrench", WrenchSelect) self.change_wrench.wait_for_service() fprint("Creating alarm listener...") self.station_hold_alarm = AlarmListener(alarm_name='station_hold', callback_funct=self.handle) fprint("Ready to go!", msg_color="green")
def connect_ros(self): ''' Connect ROS nodes, services, and alarms to variables and methods within this class. ''' # Attempts to read the battery voltage parameters (sets them to defaults if they have not been set) self.battery_low_voltage = rospy.get_param( "/battery_monitor/battery_low_voltage", 22.1) self.battery_critical_voltage = rospy.get_param( "/battery_monitor/battery_critical_voltage", 20.6) rospy.Subscriber("/wrench/current", String, self.update_operating_mode_status) rospy.Subscriber("/battery_monitor", Float32, self.cache_battery_voltage) rospy.Subscriber("/clock", Clock, self.cache_system_time) self.kill_listener = AlarmListener("kill", self.update_kill_status)
def __init__(self, timeout=0.5): self.timeout = rospy.Duration(timeout) self.last_time = rospy.Time.now() self.last_position = None self.check_count = 0 self.max_jump = 1.0 self.odom_discontinuity = False self.killed = False self.odom_listener = rospy.Subscriber('/odom', Odometry, self.got_odom_msg, queue_size=1) self.clear_kill_listener = AlarmListener('kill', self.clear_kill) self.alarm_broadcaster, self.alarm = single_alarm( 'kill', severity=3, problem_description= "Killing wamv due to unreliable state estimation") rospy.Timer(rospy.Duration(0.1), self.check)
#!/usr/bin/env python import rospy from navigator_alarm import AlarmListener rospy.init_node("alarm_test") def kill(alarm): print "KILL cleared: {}".format(alarm.clear) def rc_kill(alarm): print "RCKILL clear: {}".format(alarm.clear) al = AlarmListener() al.add_listener("kill", kill) al.add_listener("rc_kill", rc_kill) rospy.spin()
def __init__( self, port="/dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A104OWRY-if00-port0", baud=9600): self.ser = serial.Serial(port=port, baudrate=baud, timeout=0.25) self.ser.flush() self.timeout = rospy.Duration(1) self.network_msg = None update_network = lambda msg: setattr(self, "network_msg", msg) self.network_listener = rospy.Subscriber("/keep_alive", Header, update_network) self.killstatus_pub = rospy.Publisher("/killstatus", KillStatus, queue_size=1) ab = AlarmBroadcaster() self.kill_alarm = ab.add_alarm( "hw_kill", problem_description="Hardware kill from a kill switch.") self.disconnect = ab.add_alarm("kill_system_disconnect") self.killed = False self.kill_status = { 'overall': False, 'PF': False, 'PA': False, 'SF': False, 'SA': False, 'computer': False, 'remote': False } # Dict of op-codes to functions that need to be run with each code for aysnc responses self.update_cbs = { '\x10': lambda: self.update('overall', True), '\x11': lambda: self.update('overall', False), '\x12': lambda: self.update('PF', True), '\x13': lambda: self.update('PF', False), '\x14': lambda: self.update('PA', True), '\x15': lambda: self.update('PA', False), '\x16': lambda: self.update('SF', True), '\x17': lambda: self.update('SF', False), '\x18': lambda: self.update('SA', True), '\x19': lambda: self.update('SA', False), '\x1A': lambda: self.update('remote', True), '\x1B': lambda: self.update('remote', False), '\x1C': lambda: self.update('computer', True), '\x1D': lambda: self.update('computer', False), '\x1E': self.disconnect.clear_alarm, '\x1F': self.disconnect.raise_alarm } # Initial check of kill status self.get_status() self.current_wrencher = '' _set_wrencher = lambda msg: setattr(self, 'current_wrencher', msg.data) rospy.Subscriber("/wrench/current", String, _set_wrencher) al = AlarmListener("kill", self.alarm_kill_cb) rospy.Timer(rospy.Duration(.5), self.get_status) while not rospy.is_shutdown(): self.control_check() while self.ser.inWaiting() > 0 and not rospy.is_shutdown(): self.check_buffer() if not self.network_kill(): self.ping() else: rospy.logerr("KILLBOARD: Network Kill!")