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!")
#!/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()